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Abstract 


MODELING AND SIMULATION OF A STEWART PLATFORM 
TYPE PAR ALLEL STRUCTURE ROBOT 
by 

Gee Kwang Lim, Graduate Research Assistant 
Robert A. Freeman, Assistant Professor of Mechanical Engineering 
Delbert Tesar, Carol Cockrell Curran Chair in Engineering 


The kinematics and dynamics of a Stewart Platform type parallel 
structure robot(NASA's Dynamic Docking Test System) were modeled using the 
method of kinematic influence coefficients(KIC) and isomorphic transformations of 
system dependence from one set of generalized coordinates to another. By 
specifying the end-effector (platform) time trajectory, the required generalized input 
forces which would theoretically yield the desired motion were determined. 

It was found that the relationship between the platform motion and the 
actuators motion was nonlinear. In addition, the contribution to the total generalized 
forces, required at the actuators, from the acceleration related terms were found to 
be more significant than the velocity related terms. Hence, the curve representing 
the total required actuator force generally resembled the curve for the acceleration 
related force. Another observation revealed that the acceleration related effective 

inertia matrix i * dd ] had the tendency to decouple, with the elements on the main 
diagonal oft ^ dd ] being larger than the off-diaaonal elements, while the velocity 

fp* 1 

related inertia power array L ddd J did not show such tendency. This tendency 
results in the acceleration related force curve of a given actuator resembling the 
acceleration profile of that particular actuator. Furthermore, the investigation 
indicated that the effective inertia matrix for the legs is more decoupled than that for 
the platform. These observations provide essential information for further research 
to develop an effective control strategy for real-time control of the Dynamic 
Docking Test System. 
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CHAPTER 1 


INTRODUCTION 

Change, rapid change is one of the most common daily occurrences in 
today's world. Everyday there are new products cropping up in the market. It is 
practically impossible for any manufacturer to stay competitive if the manufacturing 
facilities are solely designed for a particular product, requiring months or maybe 
years to convert the facilities for production of a different product. Hence, flexible 
manufacturing is the key word in today's manufacturing environment The facilities 
must be able to adapt to changes rapidly. Besides being flexible in today's 
manufacturing world, certain manufacturing processes require an almost absolutely 
clean environment, eg., clean room operation. Furthermore, there are certain 
environments which are hazardous to human existence, such as operation in space 
and in nuclear facilities. In order to accomplish these tasks, robots, which are 
computer controlled mechanical devices capable of adapting to a wide range of 
operations, must be utilized. However, there is a limit to what a particular robot can 
or cannot do depending on the physical structure of the robot and how the robot is 
being controlled. There is nothing much that one can do to alter the physical 
structure of a robot, besides getting a different one. But, there are many ways in 
which one can improve the ability of a robot. The most obvious one is by altering 
the control strategy of the robot. For example, there are different ways for a robot 
end-effector to move to a certain location. One of the ways is to move the end- 
effector to the vicinity of the location as fast as possible and subsequently employ 
feedback control to reach the desired location. Another way is by using a more 
accurate model of the robot to predict the input loads required for a given motion of 
the end-effector and implement feedforward control in order to arrive at the desired 
location. The basis of this work then is to obtain a robot's dynamic models for the 
purpose of feedforward control. 

There are a few desirable criteria which are frequently used to 
characterize robot manipulators. They are: 


1 



I 


2 


(1) Load capacity; 

(2) Speed; 

(3) Precision. 

These three important factors are very often interrelated. Robots with 
high load capacity are usually imprecise and can only move at low speed. Or, 
robots moving at high speed are often imprecise. Hence, there is a trade-off among 
the three criteria. There are three factors that characterize the precision of industrial 
robots. The first factor is repeatability. Repeatability refers to the ability of a robot 
to return to a previously defined location in space. The second factor is absolute 
accuracy, which refers to the ability of a robot to reach a point in space defined by 
the controller. The last factor is resolution. It gives the smallest movement the end- 
effector can achieve. 

The main concern in this work is the problem of load capacity. The 
robots that are of interest here must be able to carry extremely high loads, maybe up 
to a few thousand kilograms. With this degree of load capacity, the robots 
themselves must be relatively rigid and heavy. As a result of the large mass in the 
robot structure, dynamic effects become very significant in the robot motions, 
although the robot maybe moving at a relatively low speed. There are basically two 
different classes of robots that are in use today. These are "serial" robot 
manipulators and "fully parallel" robot manipulators. Although, there can be a mix 
of the two classes, for example, the hybrid manipulator systems discussed in Sklar 
and Tesar [36], the focus in this work will be on serial and fully parallel 
manipulators. 

Theoretically, serial robots can be designed to carry high load if 
necessary. Practically, it is almost impossible to design serial robots that can carry 
high loads and maintain relatively precise motion. This is due to the fact that errors 
in serial manipulators are additive in nature. A one degree deflection in the first link 
may easily cause a significant error at the end-effector position. On the other hand, 
parallel manipulators are structurally more rigid and the error in each links is non- 
additive. Therefore, parallel manipulators are the best alternative for tasks that 
require high load capacity in a limited workspace. However, parallel manipulators 
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not only are structurally more complex, but they also require a more complicated 
control scheme. The main objective of this work is to develop the necessary 
dynamic models and computer software for future study of the generalized Stewart 
Platform aimed at formulating an effective real-time control strategy. 

This work concentrates on the development of the dynamic model for 
a fully parallel robotic manipulator based on the modeling technique called the 
method of Kinematic Influence Coefficients(KIC). The modeling procedure begins 
with the discussion on serial manipulators and then extends the technique to include 
parallel manipulators by utilizing an isomorphic transformation procedure called the 
transfer of generalized coordinates. 

To provide the readers with a better idea of the discussion that focuses 
on the development of the model for a fully parallel manipulator, Chapter 2 begins 
with an introduction on how the concept of parallel mechanisms was first 
envisioned by Mr. D. Stewart [37] and the basic design concept for this class of 
mechanism, subsequently known as Stewart Platforms. Also included is the 
application employing this class of mechanism by the National Aeronautics and 
Space Administration(NASA) for the development of the Dynamic Docking Test 
System (Gates and Graves [18], Owen and Williams [33], Strassner ([38], [39], 
[40])). In addition, a brief discussion on the different approaches adopted by 
researchers to model Stewart Platform type mechanisms is meant to give the readers 
an overview of the problems faced when using this class of mechanism. 

Chapter 3 introduces the fundamental modeling approach employed in 
this work. The approach, called the method of Kinematic Influence Coefficients, is 
based on the separation of time dependent functions and position(or configuration) 
dependent functions. This chapter develops the tools necessary for deriving the 
kinematic and dynamic models for serial manipulators. Then, using the tools just 
developed, the model for serial manipulators is formulated. 

Chapter 4 gives a detailed description of the isomorphic 
transformation technique which greatly enhances the modeling capabilities of the 
method of Kinematic Influence Coefficients. A general procedure is developed to 
transfer the kinematic and dynamic models referenced to any set of generalized 
coordinates to any other desired set of generalized coordinates via the transfer of 
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generalized coordinates. The discussion begins with serial manipulators and then is 
extended to include multi-loop parallel mechanisms. 

Chapter 5 focuses on the application of the modeling technique 
developed in the previous chapters to NASA's Dynamic Docking Test System, 
which is a variation of the generalized Stewart Platform. This chapter gives a 
detailed description of the derivation of the desired dynamic model, which can be 
used to implement feedforward control for the Dynamic Docking Test System. The 
development includes deriving the directly obtainable initial models for each leg 
referenced to their respective joint parameters and the platform model referenced to 
the platform coordinates. Also included is the transfer of the joint-based model for 
each leg to the intermediate common set of platform coordinates. And, finally, the 
transfer of the model from the platform coordinate set to the desired input 
coordinate set is discussed. 

In Chapter 6, the results of various computer simulations using the 
model developed in Chapter 5 are presented. Four different motion specifications 
and two platform trajectories are simulated. The modon specifications used are: 

(a) Class p=2, constant acceleration; 

(b) Class p=3, 3-4-5 polynomial; 

(c) Class p=4, 4-5-6-7 polynomial; 

(d) Class p=4, 3 rd derivative trapezoidal. 

From these simulations, the contribution to the overall actuator forces arising from 
the velocity-related term and the acceleration-related term are studied. Furthermore, 
the elements in the effective inertia matrix and inertia power array are also 
investigated. The magnitude of these elements in general and the relative magnitude 
of the main diagonal elements compared to the off-diagonal elements, are essential 
for studying the feasibility of real-time feedforward control. 


CHAPTER 2 


BACKGROUND 


2.1 Stewart Platform 

In the search for a suitable means for simulating flight 
conditions for the safe training of helicopter pilots, the design 
of a mechanism has been established having all the freedoms 
of motion within the design limitations of amplitude and 
capable of being controlled in all of them simultaneously. 

In the opening paragraph, as quoted above, from the 1965 publication 
by Mr. D. Stewart [37], he saw the immediate need for a mechanism suitable for 
pilot training. Serial manipulators having six degrees-of-freedom(DOF) are 
potential candidates for the job. However, the mechanism must be able to carry a 
large load and change direction, speed and acceleration within a short period of time 
to simulate flight conditions. These requirements present a serious problem for any 
serial manipulator. It was a known fact at that time, and today too, that serial 
manipulators were very limited in load capacity. Thus, using a serial manipulator 
alone to do the job is highly unlikely, so a different kind of mechanical structure is 
necessary. Mr. Stewart envisioned mechanisms which had the ability to carry a 
large load and still satisfy the above mentioned motion specifications could also be 
used in the following ways: 

(a) To simulate a space vehicle 

(b) To simulate a stationary platform on a moving ship 

(c) As a human control mechanism (man-machine interface) 

(d) As a machine tool 

(e) As an automatic assembly or transfer machine. 
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2.1.1 B asic Design Concept 

The mechanism which Mr. Stewart designed, called the Stewart 
Platform, consists of a moving platform supported by three legs through a ball joint 
at each of the connections. The other end of each leg is connected to ground 
through a two-axis revolute joint as shown in Fig. 2-1. The connections at the ball 
joints are free to rotate as the platform moves. All the legs are designed using 
prismatic joints allowing control of the individual leg lengths. One axis of the two- 
axis revolute joint is also controlled by an actuator. These inputs are referred to as 
the controlled actions in Fig. 2-1. This arrangement gives the platform six DOF 
with two DOF essentially controlled by each leg. The platform is also designed so 
that when the three legs are in their mean positions, each of the legs is contained in 
a tangential plane of a circle that passes through the three points of the platform as 
shown in Fig. 2-2. 

With this basic design concept, a wide range of applications 
previously limited by the speed and load capacity of serial manipulators will soon 
surface. For example, applications that require high load capacities will now be 
possible due to the parallel nature of the mechanism. Unlike serial manipulators 
where deflection from each link is additive in nature, any deflection under heavy 
load in a parallel manipulator is non-additive, meaning the total deflection at the 
end-effector is not the sum of each of the individual link. Another useful application 
is in the area where high precision and accuracy are critical within a limited 
operating range, eg. micromanipulator for use in micro- surgery. In this kind of 
operation, very small motions are required from the manipulator but with high 
precision, the parallel structure potentially gives the mechanism a faster and more 
precise motion than its serial counterpart. 

2-1.2 Control of the Mechanism 

As technology advances, system control becomes a vital part of 
everyday life. Take the very basic household temperature control. Without the use 
of a thermostat, one will have to perform the boring routine of switching on and off 



Freedom Axes 


Fig. 2-1 General Arrangement of the Stewart Platform* 


"Adapted from Stewart [37] 
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the air conditioner(or heater) to keep the temperature at a desired level. Opening and 
closing of the elevator door is another example of control. When someone enters 
the elevator as the door is closing, the door will stop and start to open again. These 
examples show how important control is in one's daily life, although one might not 
realize it. Control systems not only relieve humans from some easy tasks, but they 
can also facilitate the operation of a highly complex problem like sending a 
spacecraft out to an unknown galaxy. For any mechanism be of any use, the ability 
to control it effectively is essential. A mechanism will just be a show piece with no 
practical purpose if there is no way of controlling it. Hence, this section discusses 
some of the techniques suggested by Mr. Stewart for the control of the platform 
mechanism. 


2 - 1 . 2 . 1 Linear Hydraulic Actuator 

One of the methods that Mr. Stewart suggested in controlling the 
mechanism was by using two hydraulic jacks for each leg. One of the jacks is to 
control the length of the leg, while the other jack is to control the angle of the first 
jack as shown in Fig. 2-3. Also depicted in the figure is a common axis and two 
parallel axes at the base of the two jacks (two-axis joint). The common axis is not 
controlled by the leg allowing the plane containing the individual leg to rotate freely 
about that axis. This design gives the platform a three-axis motion about the ball 
joint. When the three legs are connected together at the platform, the platform 
direction, which cannot be controlled by a single leg, can be thought of as being 
controlled by the other two 2 DOF legs. Thus, the platform has a total of six DOF 
(three translational and three rotational motions). 

2- 1.2.2 Articulated Levers 

Another proposed structure uses articulated levers as shown in Fig. 
2-4. This system of controlling the platform differs from the linear actuators 
discussed in the previous section. Instead of controlling one length and an angle, 
the ball joint location within the plane of each leg, and hence, the platform is 
controlled by two angles (a and P). One advantage for controlling two angles over 



I Three-axis Joint 



Fig. 2-3 General Arrangement of Single Leg System* 

♦Adapted from Stewart [37] 




Adapted from Stewart [37] 


one length and an angle, as discussed in section 2- 1.2.1, is that larger workspace 
can be attained. By replacing the jack in Fig. 2-3 with an articulated leg, and 
operating the jack part way along the outer limb (Fig. 2-4), the amplitude of the leg 
extension can be increased as compared with the linear controlled actuator. 

2.2 Dynamic Docking Test Svstem (DOTS') 

Utilizing the basic concept of the Stewart Platform, the National 
Aeronautics and Space Administration's Lyndon B. Johnson Space Center built a 
full-scale advanced docking system in the early 1970's (Gates and Graves [18], 
Owen and Williams [33], Strassner ([38], [39], [40])). The DDTS is a large 
motion, real-time docking test simulator designed to physically accommodate the 
docking hardware of two spacecrafts. The physical configuration of the simulator is 
shown in Fig. 2-5. The simulator consists of six linear hydraulic actuators which 
support and move an active table (moving platform) on which the passive docking 
hardware is mounted during a simulation. The overhead support structure is 
supported by a strongback at one end and two vertical braced columns at the other 
as shown in Fig. 2-6. This stationary portion of the simulator supports the active 
docking hardware system, docking hardware adapter and load cell system. 

During a docking test, the lower portion of the simulator manipulates 
the passive docking hardware of one spacecraft to the active docking hardware of 
the other spacecraft, which is attached to the stationary overhead structure. Before 
the two portions of the simulator (ie., active and passive) come into contact, the 
active table is driven by the six hydraulic actuators in a preprogramed motion 
trajectory relative to the stationary active docking hardware. On contact, the loads 
sensed by the load cell system triggers the closed-loop portion of the simulation. 
These loads are then used as inputs to spacecraft equations of motion to predict the 
response of the two spacecrafts on the computer . Subsequently, real-time relative 
motion of the two spacecrafts is determined and then transformed into actuator 
motion commands. The simulation is terminated when the closed-loop load and the 
spacecraft dynamics go to zero. 
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Fig. 2-5 Perspective View of the Dynamic Docking 
Test System* 


* Adapted from Scrassner [38] 





Fig. 2-6 The Dynamic Docking Test System 



Ignoring the specific structural differences between the Stewart 
Platform of Fig. 2-1 and the DDTS of Fig. 2-5, it is apparent that the two 
mechanisms resemble each other. Both the mechanisms have a moving platform 
that is supported by some mechanical linkages (or legs) and they are parallel in 
nature. By controlling the orientation and/or length of the legs, the motion of the 
platform is then defined. The only difference between the two mechanisms is that 
the platform in Fig. 2-1 has three legs while the platform in Fig. 2-5 has six legs. 
However, both the mechanisms still provide control of the six DOF of the platform. 
Two DOF are controlled by each of the leg in the arrangement in Fig. 2-1, but only 
one DOF is controlled by each of the leg in the design shown in Fig. 2-5. 

The initial use of the DDTS was for the Apollo/Soyuz Test Project, 
which was an international mission between the United States and the USSR. The 
project involved the docking of Apollo and Soyuz docking modules in the space 
orbit. The high load capacity requirement from the two docking modules 
discouraged the use of serial test mechanism due to the additive nature of the 
deflection from each of the serial linkages. Future applications of the DDTS will be 
for the space shuttle and the space station. 

2.3 Survey of Related Work bv Other Researchers 

In the last two decades, science and technology have advanced 
tremendously. Computer technology for instance, has reached a point where 
products produced just one or two years ago are considered outdated. With this in 
mind, one would expect the same impact on robotic technology since the computer 
is the brain of the robot. Unfortunately, this is not true. Only recently has industry 
seen the potential for robots in the manufacturing environment. This delay in 
realizing the need for robotics technology created a gap between what the brain of a 
robot is capable of doing and what the robot is physically able to do. Although there 
is a lot of robotics research ongoing, most of the emphasis is in the area of serial 
manipulators, from which most industrial robots are derived. A very limited amount 
of effort is put into the development of parallel robotic mechanisms. A survey of the 
literature will illustrates this trend in the engineering research community. After an 
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extensive survey, only the following publications are available: Callan [4], Cox [6], 
Cwiakala [8], Do and Yang [9], Fichter [12], Freeman and Tesar ([14], [15]), 
Hudgens [21], Hunt [22], Marco [28], Mayer and Wood [30], Mohamed and 
Duffy [31]. This limited effort maybe due to the complexity of the geometry as 
compared to serial linkages or, it maybe because not many researchers have yet 
realized the potential of parallel mechanisms. 

In the following, a few of the recent publications from researchers 
across the country covering the different aspects of parallel mechanisms will be 
discussed. The first report investigates the dynamics of the platform type 
mechanism using the Newton-Euler formulation. Following that is a report that 
investigates the workspace of the mechanism. The third report deals with the 
general theory and practical construction of the mechanism. While the last 
investigation is to utilize the mechanism as a micromanipulator for delicate force 
control, error compensation and fine manipulation. 

In a publication presented by Do and Yang [9], Newtonian mechanics 
was used to calculate the actuating forces for the actuators of a Stewart Platform 
shown in Fig. 2-7. It is found that the dynamics of the mechanism is governed by 
thirty-six simultaneous equations. Instead of solving the entire set of simultaneous 
equations, which would be computationally demanding, it is also found that the 
thirty-six equations can be arranged to form systems of six linear equations. The 
approach taken in the reported research is very "conventional". By conventional it is 
meant that the method is widely used in the engineering community. As a result, it 
is easily understood by most researchers. However, this approach lacks generality 
in the sense that there is no general rule as to how to form the six desired equations. 
A complete analysis of the mechanism may be necessary to obtain a similar set of 
six equations if a slightly different model is used. 

Another publication presented by Cwiakala [8], analyzed the 
kinematics of the platform. Using only the kinematic model, the workspace of the 
Stewart Platform was studied. By utilizing the special symmetry of the platform, 
the investigator found that the problem of determining a representative workspace 
cross-section could be reduced to a planar problem. Also addressed in this work is 
the development of an efficient approach to generate the workspace of the platform. 



It is found that the workspace cross-section of the Stewart Platform under 
consideration is nonsymmetric. In addition, the nonsymmetry and the volume 
increase as the ratio of the lower radius (base) to upper radius (platform) increases. 
This implies that by increasing the radius of the platform, the workspace can be 
increased. 

A different approach was taken by Fichter [12]. Screw theory was 
used to determine the dynamics of the Stewart Platform. Unfortunately, the author 
of this work is not familiar enough with the technique of screw theory to contribute 
any useful comment. However, one of the Findings which has great interest to this 
author is the determination of singular positions for the mechanism. It is noted that 
instead of losing one or more degrees of freedom at the singular positions, as one 
does with serial manipulators, parallel manipulators gain one or more degrees of 
freedom. By this it means that the control of one or more degrees of freedom of the 
platform is lost. The singular positions of the Stewart Platform similar to the one 
shown in Fig. 2-5 or Fig. 2-7 occur when the six lines of action of the forces of the 
legs are linearly dependent. This condition can also be found by calculating the 
determinant of the matrix of the Plucker coordinates. At singular positions, the 
determinant of the matrix becomes zero. For detailed results of this investigation the 
readers are referred to the publication. In addition, Mohamed and Duffy [31] also 
investigated the first-order properties via screw theory. 

Applying the concept of the Stewart Platform, researchers have come 
up with many applications for parallel robotic mechanisms. One of the very 
practical uses is as a micromanipulator. It can be used alone for very small motion 
and high precision operation or it can be combined with a serial manipulator to take 
advantage of the positive aspects of both the parallel and serial manipulators. By 
positive aspects the author means the relatively high speed and precision of the 
parallel structure, and large workspace volume together with the relatively high 
dexterity of the serial structure. Sklar and Tesar [36] investigated the kinematics and 
dynamics of hybrid serial/parallel manipulator structures. 

Hudgens [21] investigated a fully-parallel six DOF micromanipulator 
in his Master's Thesis. The micromanipulator investigated there, shown in Fig. 
2-8, is a variation of the generalized Stewart Platform. Instead of varying the link 
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Fig. 2-8 Kinematic Representation of the 
Micromanipulator Mechanism* 


Adapted from Hudgens [21] 
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length of the six legs to control the platform, it is controlled by varying the posiuon 
of the lower spherical joints in a circular arc within the base plane. The rotary input 
in Fig. 2-8 controls the movement of the lower spherical joint via the base link. The 
motivations for using the four-bar linkage at the active input to control the platform 
instead of the link length are the following: 

(a) only a small workspace is necessary in this particular application. 

(b) this input arrangement provides high resolution at the platform. By 
high resolution it means large input motion to small platform 
motion. 

(c) the fixed link provides higher load capacity compared to prismatic 
link. 

One simplification made in his research is to replace the lower 
spherical joints by two DOF hooke (universal) joints, since the spin of each leg 
about its own axis does not contribute to or affect the input/output relationship of 
the mechanism. This simplification is shown in Fig. 2-9 for one branch. The 
modeling technique employed by Mr. Hudgens is very similar to the method used 
in this work. However, numerous modifications and variations are made and will 
be highlighted as they appear in the following chapters of this work. 

This chapter is only intended to provide a quick and brief overview of 
the past and present works in the area of parallel robotic mechanisms. It in no way 
represents all the research and advances in this area. However, deriving from the 
limited resources and knowledge, the author strongly feels that a lot of research is 
needed in order to fully understand and exploit the potential of parallel mechanisms. 






CHAPTER 3 


KINEMATIC AND DYNAMIC MODELING USING 
KINEMATIC INFLUENCE COEFFICIENTS 

3-1 Overview of The Method of Kinem atic Influence Coefficients 

Section 2-3 reviewed some of the techniques employed by a few 
researchers for the kinematic and dynamic modeling of six DOF parallel robotic 
mechanisms. This chapter will discuss a technique not very widely used in the 
engineering community but which has been used with great success by a group of 
researchers at the University of Florida(now at The University of Texas at Austin) 
for the past few decades. The technique has been termed the method of Kinematic 
Influence Coefficients(KIC). The basis of this work stems from the method of KIC 
continually developed over the years by researchers including Tesar, Benedict, 
Thomas and Freeman. The present chapter will concentrate on the discussion of 
serial manipulator modeling using KIC. In the next chapter, discussion will focus 
on how to apply the model of a serial manipulator to a parallel manipulator. This 
ability is largely a result of the generality and versatility of the KIC approach. 

The method of KIC is based on the separation of time dependent 
functions and position(or configuration) dependent functions. Throughout the entire 
process of obtaining the kinematic and dynamic models, the formulation will strictly 
adhere to this fundamental concept, keeping the two terms separated. Following 
this concept, a lot of research has been done and well documented in the literature. 
Some of the more significant findings are : Benedict and Tesar ([1], [2], [3]), 
Thomas and Tesar ([42], [43]), Freeman and Tesar ([13], [14], [15]). The 
technique was initially developed by Benedict and Tesar to analyze planar 
mechanism. In 1982 Thomas and Tesar took the approach one step further by 
developing procedures for the analysis of a general serial manipulator. Freeman and 
Tesar through the years 1982 to 1986 extended the applicability of the approach by 
developing the technique of generalized coordinate transformation, which will be 
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discussed in the next chapter. This greatly enhances the potential power of KIC in 
dealing with a larger and more general class of mechanism and forms the analytic 
basis of this work. 

To aid readers who are unfamiliar with the method of KIC and the 
notational scheme employed in this work, it is essential to introduce the readers to 
the notation used in the present work. The notational scheme adapted here was 
developed by Freeman and Tesar [14]. Although part of the notation may appear 
redundant at the stage when dealing with a single set of independent generalized 
coordinates, it will be shown to graphically enhance the manipulation process for 
the transfer of generalized coordinates, which will be discussed in the following 
chapter. The basic formulation of this scheme involves a square(or block) 
arrangement with the central block surrounded by both pre- and post- superscripts 
and subscripts at the four comers as illustrated in Table 3-1. The square is divided 
into two portions with the top half reserved for dependent system parameters and 
the bottom half reserved for independent system parameters. The center of the 
square is reserved for a symbol representing a system parameter (e.g., a set of 
generalized position parameters (n)), a physical quantity (e.g., a generalized force 
(D), or a mathematical operation (e.g., first-order partial geometric derivative (G)). 
Post super- and sub-scripts denote which system parameter is involved with the 
center symbol. Pre super- and sub-scripts give any additional information that 
might be helpful to describe the system parameter. Hence, at times the pre scripts 
maybe missing from the square. Matrices and higher dimensional arrays are 
denoted by square brackets (ie. [ ]) enclosing the symbol along with the super- and 
sub-scripts. Although the notational scheme may appear complex and confusing to 
first time readers, when one becomes familiar with its usage, the advantages of this 
scheme will soon surface. One will greatly appreciate the graphically descriptive 
information provided in the square when dealing with the transference of system 
dependence from one set of generalized coordinates to another. 

Also, additional notation is used when dealing with higher 
dimensional arrays and their subsets. When four dimensional arrays are used in this 
work, the first index represents the leg number associated. Unfortunately, the first 
index of a three dimensional arrays may represent the leg number or plane 
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associated. However, only in the simulation software are four dimensional arrays 
used with the first index indicating the leg number. Otherwise, throughout the 
derivation, only up to three dimensional arrays are used with the first index 
indicating the plane number. The last two indices represent the row and column, 
respectively, of a matrix or the matrix of one of the planes. In other words, the 
indices for four dimensional arrays, from left to right denote: leg; plane; row; 
column. For three dimensional arrays, the indices denote: leg/plane; row; column. 
Table 3-2 gives an example of the notation used. The subset of an array is also 
indicated by its indices. A missing index implies that all the components in that 
dimension are present in the subset of the array. For example, an array with indices 
like this: k; ;m;n, represents the m 1 * 1 row and n 1 * 1 column of all the planes for the k* 
leg. This is also illustrated in Table 3-2. 

The next two sections will focus on the development of a general 
approach for the kinematic and dynamic modeling of serial manipulators, based 
mainly on Freeman and Tesar [14] and Hudgens and Tesar [21]. Due to the fact that 
all parallel mechanisms can be viewed and modeled as a combination of serial 
mechanisms, it is essential that the readers fully understand the derivation that will 
be discussed in the following sections. Besides, almost all of today's industrial 
robots are serial in nature, hence, the method discussed here can be used to analyze 
almost any robot in use and to aid in the design of robots. 


3-2 General Approach to Develop the Kinematic Model of Serial 

Manipulators 


Consider a set of M-dimensional time dependent motion parameters, 
eg., the vector (H), written as 




-iT 


u'(t),u 2 (t),u 3 (t),..., u M (t) 


(3-1) 


where the superscripts( 1,2,3,. ..,M) denote which of the system parameters are 
involved in the description of the kinematic state. The superscript T denotes the 
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Example of the A array with indices like this : 2; ;1;1, represent 



For three dimensional array A, 



Table 3-2 Indicial Notation for Higher Dimensional 
Array 
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transpose of the row matrix. It is also assumed that H is a function of an N- 
dimensional set of generalized coordinates 




-.T 


<Pi (t),cp 2 (t),cp 3 (t),...,cp N (t) 


(3-2) 


where the subscripts denote which of the generalized coordinates are involved. As a 
result, the system can be expressed parametrically as 


u m = u m (( 2 ) ; m = 1,2,3,. ..,M 
and 

<p n = cp a (t) ;n= 1,2,3,..., N. 


(3-3) 

(3-4) 


Using the above notation, the first order time derivative of ^ is 
du 9]i 3<e 


dt 


9<g 


It 



(3-5) 


By defining the first order kinematic influence coefficients, which will be referred 
to as the "G-function" throughout this work (as opposed to the more common use 
of the term Jacobian), as 

g;]-^ 

(3-6) 


equation 3-5 can be expressed as 




<2 


(3-7) 


where 


r u l 

. is a M by N matrix and ^ is a N by 1 vector. 

The second order time derivative of H is a direct differentiation of li , 
which is expressed as 
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•• 3u •• d fdn\ • 
or 

Applying the chain rule to the time derivative of [ G< ^] gives 

* L ,J dt [ a J 


= _a_/au 

5cg 1 


Substituting equation 3-5 into equation 3-9 and regrouping gives 


SM-fsU); 


< _d_ 
\9(g 




(3-8) 


(3-9) 


(3-10) 


By definition, the second-order kinematic influence coefficient is the partial 
derivative of with respect to which will be denoted throughout this work as 



(3-11) 


or as the "H-function". The H-function is a M by N by N array. Substituting 
equation 3-11 into equation 3-10 and arranging for dimensional compatibility with 
equation 3-8 gives 



(3-12) 


Substituting equations 3-6 and 3-12 into equation 3-8 gives 
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(3-13) 


Recalling equations 3-7 and 3-13 and the method of derivation from equation 3-5 to 
3-13, as far as possible, the expressions follow the fundamental principle of 
separating the positioner geometry) dependent terms from the time dependent 
terms. Also notice that both the G- and H-functions are purely position dependent. 
Equations 3-7 and 3-13 are the two basic formulations which will be used in this 
entire work for the velocity and acceleration terms. Freeman and Tesar [14] carried 
the derivation further to include the third order time derivative of U, which is the 
jerk. However, this will not be discussed in the present work. 

Before going any further, it is important to explicitly define the 
components that make up the G- and H-functions. Since 

<>:]-* 

(3-6) 


[ H "]"(i»[ G ’]) 

the G-function can be written as 
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(3-11) 
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du* du* du 1 

5(pi 5<P 2 3 <Pn 

du 
9<p, 


- M _ M 

du du 
a<p, a<p 2 


du™ 

a<p N 


(3-14) 


ra r _ ui 

where § n is defined as the m th row and n^ 1 column of the M by N IV*J matrix and 

m = 1,2,...,M and n = 1,2,...,N. The H-function can be written as 


h ii h t2 ... h 1N 



l^Nl ^N2 ••• l^NN 


dV aV 

3cp 1 9cp 1 9cp , 9<p 2 
d u 

a(p 2 a<Pi 


* 

a u 

3<pj 9<p N 


^ 2 i ^ i 

d u d u 
a<p N 9<p, d<p s dy 2 


d u 

a<p N a<p N 


(3-15) 


where i = 1,2,. ..,M. Note, * is defined as the component of the i^ 1 plane, j^ 1 row 
and k ^ 1 column of the M by N by N, [^<p <p] array. 



Until this point, none of the derivations pertain to any particular 
manipulator. However, they provide the necessary tools for the development of the 
next few sections and the chapters that follow. 

3-2.1 Kinematics of Serial Manipulators 

The kinematic analyses of serial manipulators has been treated 
extensively by researchers including Colson and Perreira [5], Craig [7], Freeman 
and Tesar ([13], [14], [15]), Fu, Gonzalez and Lee [17], Lee [24], Lee and Ziegler 
[26], Paul [34], Paul, Shimano and Mayer [35], Takano, Yashima and Yada [41], 
Thomas and Tesar ([42], [43]), and many others. This section is designed 
specifically for the benefit of those readers who are unfamiliar with the previous 
mentioned works or for those who are unfamiliar with the notational scheme 
adopted by Thomas and Tesar ([42], [43]) or Freeman and Tesar ([13], [14], [15]). 
It is only intended to serve as a brief review of the current topic. 

An N DOF serial manipulator, as the name implies, consists of N 
links and N joints connected together in series(ie., one link after the other). Only 
two of the lower-pair connectors, revolute and prismatic joints, are considered in 
this work due to the fact that any other joint can be treated as a combination of the 
two. A general N DOF serial manipulator is shown in Fig. 3-1, as adapted from 
Freeman and Tesar [14], Each rigid link in the serial chain is characterized by four 
independent parameters according to the Denavit-Hartenberg convention. They are 

the link offset( ^jj or Sj ) ? joint angle( ® jj or ®j ), link length( a i <M)) and twist 

angle( a JXj*n ). A double subscript here denotes a fixed system parameter, whereas, 
a single subscript represents a system variable. Following this scheme, a revolute 
link will have ^jj and ®j as the two independent joint parameters. On the other 

hand, and u will be the two joint parameters for a prismatic link. The 

coordinate system adopted here sets the base link(or link 01) coincident with the 

• • • 

fixed axes( X, Y, Z ^ with the first axis of rotation (or translation) 2i defining the 

V 12 V 0 0 

direction of ^ . 2 is along the fixed axis A when ‘(or ") is zero. The pre- 

(J) Y <j)_ -D j 

superscript with a bracket in A and ^ represents the local X and Z axes. is 


£ 3 , ,3, 2 



Fig. 3-1 Kinematic Representation of the 
Serial Manipulator* 


‘Adapted from Freeman and Tesar [14] 


the vector pointing from the origin of the fixed reference frame to the origin of the 
jth link frame. This vector can be expressed as a sum of all the preceding vectors 3 
and £ as 


34 


R j 


i u, ( o-o i 
SnS + 2 ^ 3 + 



1-2 


(3-16) 


Following this convention for setting up the local link frames, the link parameters 
can be defined as: 


(a) Link offset °jj or = the distance from 3 to 3 measured 

j 

along 3 direction; 

a o 0-1) j j(j*l) 

(b) Joint angle jj or j = the angle between 3 and 3 

j 

measured about 3 axis; 
a J j-i ■ 

(c) Lmk length > <j +1 > = the distance from 3 to 3 measured along 

3 direction; 

fY j 

(d) Twist angle jtj*» = the angle between 3 and 3 measured about 

3 axis. 


3-2. 1 . 1 First-order Kinematics 

General rigid body motion can be conveniently expressed in terms of a 
translational velocity component and a rotational velocity component. These two 
velocities can be separated by treating the body as a point to obtain the translational 
term, and as an object rotating about that point to get the rotational term. Serial 
linkages are made up of rigid bodies connected together sequentially. In order to 
determine the velocity at any point in the chain, one needs to first find the angular 
velocity of the link frame containing the point and then the velocity of the origin of 
that frame. 



Rotational. By the angular velocity addition theorem, the angular 
velocity of link jk in Fig. 3-1 can be expressed as the sum of all the relative angular 
velocities of the links preceding link jk, ie. : 



m - 1 (3-17) 

• m 

where y m * is the relative angular velocity between link (m- 1 )m and link m(m+ 1 ) 

for a revolute joint and equals to zero for a prismatic joint, since m is zero. 

Following the technique used to derive equation 3-7, equation 3-17 is separated into 
m A 

the geometric term S. and the time dependent term m , and expressed as 


jk 


£2 



(3-18) 


Comparing equations 3-17 and 3-18, it can be shown (Freeman and Tesar [14]) 
that, 



, m< j ; cp m = 8 m for revolute joint 
, otherwise 


(3-19) 


Recall in Section 3-1 that represents all the rows in column m of matrix 

[^*1, which is the unit vector & of joint axis m expressed in terms of the fixed 

• * • 

cartesian reference frame ( X, Y, j e : 


ra 

i 




(3-20) 


There is no angular velocity contribution from prismatic joints, therefore, the zero 
vector for all other cases, including j > m. 
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Translational. The translational velocity of a point P fixed in link jk 
can be derived by taking the time derivative of the position vector of point P in the 
fixed reference frame. The position vector of P can be written as : 

(3.21) 

where is as defined in equation 3-16, J is the rotation matrix relating link 

frame j to the fixed coordinate frame. The pre-superscript with a bracket represents 

au . 

a locally referenced vector or component, therefore, hi is the position vector of P 
with respect to the origin of frame j and expressed in the local frame coordinates. 
Taking the time derivative of equation 3-21 gives 

. . t L f . . ra . m 

E = Si S + s mm S + Sm s 

m«2 

(3-22) 

It can be shown (Freeman and Tesar [14]) that simplifying equation 3-22 gives a 
more compact form as 

(3-23) 

where x denotes a vector cross product. Equation 3-23 gives the necessary form 
to separate the position dependent terms from the time dependent terms. As a result, 
equation 3-23 can be expressed in the desired form 

J E = [ G »]i (3-24) 



where 




i m x ( J £ - E m ) , m < j ; cp m = 0 m revolute joint 
C , m< j ; (p m = s m prismatic joint 

Q. , m> j 


(3-25) 


Equation 3-25 simply states that the G-function of joint m is the vector cross 
product of its unit vector along the axis of rotation with the vector pointing from the 
origin of the m* link frame to the point P, if link m is revolute. In the case of a 
prismatic joint, the G-function is simply the unit vector along the axis of motion. 
Otherwise, it is the zero vector (ie., m > j ). 

3-2. 1.2 Second-order Kinematics 


The reader is referred to the development of equation 3-13 in the 
beginning of section 3-2, since the derivation of the second-order kinematics here is 
based heavily on that section. 


Rotational. The angular acceleration of a rigid body can be obtained by 
simply differentiating equation 3-18 with respect to time. This yields 


jk . jk 

CL = 



(3-26) 


Recalling equation 3-12 



(3-12) 


The second term in bracket in equation 3-26 can then be expressed as 



(3-27) 


Substituting equation 3-27 into equation 3-26 gives 
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^ k UJXI •• • T r TT jk i • 

QL =[G, J(g+ (g [H„J$ 


(3-28) 


:[»«] iS 


where L n< P <pJ is a 3 by M by M array with its i 111 plane corresponding to the i^ row 

jk 

of the vector & . 

Recall equation 3-19 for the rotational G-function. Taking the time 
derivative of equation 3-19 results in 


£[< 4 -= 


i , m < j ; cp m = 0 m for revolute joint 
Q , otherwise 


Since 


. m 

i = 


^ m-I* i 

24,1 


i-1 


x £ 


using the relationship 




the non-symmetric rotational second-order influence coefficients are 

[ jkl _ ; m<n ^j l <Pm=6m> 90=00 

1 Q ; otherwise 


(3-29) 


(3-30) 


(3-31) 


(3-32) 


fw jlt l [H jk l 

;m;n i s ^e vector component of the array L , which can be viewed as a 3 by 
1 vector running into the three planes of at m* row and n 111 column of the 
planes. The i 1 * 1 row in [^o<p] ;m;n corresponds to the i* plane in J. 


Translational. The derivation for the translational acceleration of a 
rigid body is more involved than the angular acceleration described above. This 
author will not give the detailed derivation but rather just the final result. Readers 


who are interested in the derivation are urged to refer to Freeman and Tesar [14]. 
However, the general procedure is still similar to that of the angular acceleration. 
By taking the time derivative of equation 3-24, the result is 



(3-33) 


Using the general result of equation 3-27 gives 

J E = [ J Gj]i+i [ tojji 


(3-34) 


where 



¥• 

■A 

, m < n < j 

; R R 


■r1 

, n < m < j 

; R R 



, n < m < j 

; P R 



, m< n < j 

; R P 



, m< n < j 

; P R 



, n < m< j 

; R P 



, m or n > j 

; all 


(3-35) 


The first capital letter at the end of each row represents the joint type of m, the 

second one represents the joint type of n; R stands for revolute joint and P stands 

for prismatic joint. Notice that the form of equation 3-35 gives a symmetric matrix 
[ VrP 1 

for each plane of L H, which is different from the non-symmetric planes of the 
second-order rotational influence coefficients. The results for all the G- and H- 
functions are listed in Table 3-3 and Table 3-4, respectively. 
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Table 3-3 First-order Kinematic Influence Coefficients for 
Serial Manipulators 
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Table 3-4 Second-order Kinematic Influenc 
Coefficient for Serial Manipulators 



3-2. 1.3 


Forward Kinematics 


4 2 


After determining the first- and second-order kinematic influence 
coefficients of a serial manipulator, we are ready to investigate the forward 
kinematics (ie., position, velocity and acceleration) of serial manipulators. In a 
practical situation, rather than specifying the state of the joints, often called the joint 
space, usually the kinematic state of the end-effector, which is often called the 
Cartesian space, is specified. It is frequently trivial to solve for the forward 
kinematic state of any manipulator. Given the kinematic state of each of the joints, 
the position and orientation of the end-effector can easily be determined by using 
any of the fundamental geometric or algebraic approaches (Craig [7], Fu, Gonzalez 
and Lee [17], Paul [34]). This will not be discussed in this work. However, in 
order to aid those readers who are unfamiliar with the use of the G- and H- 
functions, the forward kinematic analysis for velocity and acceleration will be 
briefly reviewed here serving as a prelude to the reverse kinematics that will be 
discussed in the next section. 

Consider a general six DOF serial manipulator with a point P fixed in 
the last link (or the end-effector). Knowing the kinematic state of each of the joints, 
represented by the symbol ^ as the generalized joint inputs, the kinematic state of 
the end-effector, represented by H, can then be determined. ^ is a 6 by 1 vector for 
a six DOF manipulator, and H is always a 6 by 1 vector. The resulting forward 
kinematic model is 



(3-36) 


and 

ii = [G“]j + i 

where the first three rows of L and U are the translational terms, and the last three 
rows are the angular terms. The combined 6 by 6 G-function is then given by 




where the top consists of the 3 by 6 translational G-functions and the bottom 
contains the 3 by 6 rotational G-functions. The H-function is formed as 


and 


[h u 4, = [ 6 h:J i;; ; i- 1,2,3 
[h4 : = [h 4; : ; i-4,5,6 


(3-39) 

(3-40) 


The combination of equations 3-39 and 3-40 gives a 6 by 6 by 6 array with its first 

three planes(equation 3-39) corresponding to the first three rows of A! for the 

. .6 

translational acceleration £ , and the last three planes (equation 3-40) corresponding 

67 

to the last three rows of AA for the angular acceleration & . 

3 - 2 . 1 . 4 Reverse Kinematics 


Having been introduced to the application of the G- and H-functions 
for the forward kinematics, this section will discuss the more operationally difficult 
problem of reverse kinematics. This involves much more complex solutions and 
physical considerations. Before solving the reverse kinematics problem, it will be 
beneficial to discuss the complexity of the soiution(s). 

There are three main concerns with the reverse kinematic solutions 
(Craig [7]). The first concern is the existence of solutions. Do the solutions exit? 
This raises the question of manipulator workspace. There are two kinds of 
manipulator workspaces. One is called the dextrous workspace, which is defined as 
the volume of space within which the end-effector can reach in all directions. The 
other is called the reachable workspace, which is defined as the volume of space 
that the robot can reach in at least one direction. Hence, the specification of the end- 
effector must be within the possible workspace of the robot. 

The second concern is the problem of multiple solutions. Unlike 
forward kinematics which yield a unique solution, reverse kinematics can have 
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more than one solution with the same end-effector specification. This requires the 
selection of one of the possible solutions. The robot controller must be capable of 
making this kind of decision. For example, to go from point A to point B in Fig. 
3-2, the robot has the choice of two configurations, I and II . The robot has to 
decide which is the more efficient way to take and if there is an obstacle, which is 
the best way to avoid it. 

Being able to resolve the previous two concerns still does not 
guarantee a satisfactory solution. There is the problem of singularity. If the 
specification for the end-effector puts the manipulator in a singularity configuration, 
the robot must be able to foresee the problem and work out an acceptable alternate 
solution. 

Knowing the problems related to reverse kinematics, the next 
discussion will concentrate on deriving the solution(s). Given the end-effector 

states( If, II and H ), it is desired to determine the state of all the joints( % ^ and ^ ) ; 
where 


(p = 9 ; for revolute joint 

and 

cp = S ; for prismatic joint 

From equations 3-36 and 3-37, we can arrive at the following equations 




.. . T 

H- (E 




(3-41) 

(3-42) 


Substitute equation 3-41 into equation 3-42 gives 

5 = [G;] ‘ii - [a;] '( ii T [G;] T [H;.] [g:] ‘ d 


(3-43) 
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Using the method of generalized dot product( Appendix A), defined by Freeman and 
Tesar[14], the second-order reverse kinematic state is given by 


5 = i T [o'»]' T ( [g;[‘. [h; J )[g;]" , 



(3-44) 


However, there is still one serious problem, and informed readers should ponder 


for a minute and ask the question how to determine L VJ *J and L’^J. Recall in 


and 


Section 3-2. 1.3, and are functions of the joint inputs Thus, before 
arriving at equations 3-41 and 3-44, ^ has to be found. There are many different 
approaches which yield solutions for the joint inputs given the end-effector position 
and orientation. Interested readers are strongly urged to refer to Duffy [11] as 
reverse position analysis will not be discussed here. 

Equations 3-41 and 3-44 are most useful in the context of this work, 

of course, provided thatt^*] and are already known. The two equations give 
a systematic and general approach to solving the first- and second-order 


geometric(and time) derivatives of the joint parameters(^) in terms of the end- 
effector parameters (ii), which are among the most essential elements in determining 
the cartesian based robot dynamic model. The first- and second-order KIC for serial 
manipulators with revolute and prismatic links are listed in Table 3-3 and Table 
3-4. Freeman and Tesar [ 14] expand the derivation to third-order KIC, which again 
will not be discussed here. 


3-3 General Approach to Develop the Dynamic Model of Serial 

Manipulators 

So far this investigation has focused on the study of kinematic 
considerations^., velocity and acceleration) of serial manipulator. Nothing has 
been said about the generalized forces or torques associated with the rigid body 
motion. The determination of the forces or torques that are necessary to cause the 



desired motion is another essential question to be answered in studying any 
manipulator (Greenwood [19], Hollerbach [20], Kane and Levinson [23], Lee, Lee 
and Nigam [25], Takano, Yashima and Yada [41], Torby [44], Tourassis and 
Neuman ([45], [46]), Walker and Orin [47]). This section will discuss the 
development of the dynamic model of serial manipulators. 

The derivation of the manipulator dynamics in this work is based 
almost entirely on Freeman and Tesar [14]. The two fundamental principles of 
mechanics employed in the derivation are the principle of virtual work and 
d'Alembert’s principle. Similar to the kinematic model described in the previous 
section, the form of the dynamic equations is also expressed in terms of 
configuration dependent and time dependent terms. This separation greatly 
facilitates the transfer of generalized coordinates employed in developing the 
dynamic model. 

The dynamic model developed will be separated into two parts. The 
first part is the effect of applied loads on the manipulator generalized input loads by 
using the principle of virtual work. Then, d'Alembert's principle will be applied to 
study the effect of the manipulator's inertia. The combination of these two parts 
enables one to express a highly geometric form of the dynamic model for a 
manipulator. 


3-3.1 Applied Loads 

Noting the simplicity of the robot kinematic expressions that result 
from separating the rotational and translational KIC, the derivation of the dynamic 

model of serial manipulators will follow this preferred scheme. Consider a M DOF 

j f p 

serial manipulator in space with a set of M, 3 by 1 force vectors I along with a set 
of M, 3 by 1 moment vectors HI (j=l, 2, ..., M and k=j+l), acting respectively on 
point P and link jk. Using the principle of virtual work, the set of M generalized 
input loads required to offset the applied loads and keep the manipulator in static 
equilibrium can be determined. The virtual work done by the applied loads is 
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sw L = jr {( j f P ) T [ J G:] + (m ik ) T [G; k ]}i5t 

j-l 

and by the input loads is 

5W, =(IJ T ffi5t 


(3-45) 


(3-46) 


According to the principle of virtual work the total virtual work done on the system, 
which must be zero for equilibrium, is 


5W = 5W L + 8W,= 0 


(3-47) 


or, substituting equations 3-45 and 3-46 into equation 3-47, 


5W - ji {( ‘£') T [ 'gJ]*( m ,l ) T [c;‘]} + 1 1.) T ] i St - 0 


(3-48) 


Equation 3-48 implies that 

j_1 (3-49) 

t l • 

where J -<f> is defined as the effective input loads. Notice that the effective input load 

has opposite sign from the required offset input loads The G- and H- 
functions are as defined by equations 3-19 and 3-25. 


3-3.2 Inertial Loads 


Consider the same M DOF serial manipulator as in Section 3-3.1. 


Defining the locally referenced inertia tensor as L 
momentum of link jk is 


(j) jk 

n 


the local angular 


(j) jk T (j) jk l (j) ; 

L =[ n J fij 


(3-50) 
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0 ) jk 

where ^ is the absolute angular velocity of link jk expressed in terms of the 
local link frame. To express equation 3-50 in the fixed referenced frame, simply 

premultiply the equation by a rotation matrix [ ^ 1 This gives 


LMTt'lMT'jj V] "W 

Since 

jk r n (j) jk 

fli =[T J J a 


(3-51) 

(3-52) 


rewriting equation 3-52 gives 


(j) jk r j-1-1 jk 

& = [ T J (3-53) 

Also, the inverse of the rotation matrix [ ^ ] is its transpose due to its orthonormal 
property. Therefore, substituting equation 3-53 into equation 3-51 gives 


l j,c =[t j ] V[t j ] 


t jk 


(3-54) 


Notice that the angular momentum is now expressed in the fixed reference frame. 
By defining the globally referenced inertia tensor as 


jk] r . 

n J = [ t j 


if a) jlc 1r ji 1 

Jl n J[t J 


(3-55) 


equation 3-55 can be written as 



jk] jk 

n sa 


(3-56) 


Before continuing with the inertial load derivation, there is one more term that needs 
attention. It is the mass center of each link, which is located by 

Jk £ = K j + [T J ] (j) £ 


(3-57) 



where & is the vector from the origin of the fixed reference frame to the origin of 

(J) r 

link jk local reference frame. is the vector pointing from the local origin to the 
center of mass expressed in the local frame. The linear velocity and acceleration of 
the link center can be found by taking the time derivative of equation 3-57. The 
results are in the same form as equations 3-24 and 3-34 for the velocity and 
acceleration, respectively, due to the similarity between equations 3-21 and 3-57. 
This gives the velocity of the center of mass as 



(3-58) 


and the acceleration as 




) e 
H 


9<f 


IS 


(3-59) 


Finally, applying the generalized principle of d'Alembert, which is the 
combination of the principle of virtual work and the principle of d'Alembert, 
together with the Newton-Euler equations of motion gives the total effective inertial 
load as 




jk jk 
& +Q X 




(3-60) 


Equation 3-60 correctly describes the effective inertial loads, but it is 
not in the desired form. Recall the fundamental rule in this work is to separate the 
positioner configuration) dependent terms from the time dependent terms. Freeman 
and Tesar [14] showed that the last term in equation 3-60 can be expressed without 
the cross product (in a quadratic form) as 



5 1 



(3-61) 


where 



Now, substituting equation 3-28 for & and equation 3-18 for ^ 


(3-62) 
along with 


equation 3-61 into equation 3-60 gives 




(3-63) 


Rearranging gives 
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<2 



Utilizing the generalized dot product mentioned in the previous section, equation 
3-64 can then be expressed as 


where 


and 



< 2+<2 




(3-65) 


(3-66) 



(3-67) 


Notice that equation 3-65 continues to separate the configuration dependent terms 
from the time dependent terms. The form of equation 3-65 is most desirable for the 
dynamic model of the effective inertial load of serial manipulators, in this context. 
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3-3.3 The Dynamic Equations 


Having determined the equations for the applied and effective inertial 
loads, the dynamic equations for a general M DOF serial manipulator can then be 
expressed as 



(3-68) 


Equation 3-68 is expressed in a very compact and explicit form, yet not losing the 
fundamental rule of separating the configuration dependent terms from the time 
dependent terms. As a result of this formulation, modeling a serial manipulator can 
be broken down into the following steps: 

(a) set up the coordinate frame for each of the links according to the 
convention adopted in Section 3-2.1; 

(b) specify the desired position and orientation of the end-effector; 

(c) perform the reverse position analysis to determine all the joint 

variables ( ® j for revolute joint and for prismatic joint ), and the 
location of the center of mass for each link; 

(d) if necessary, multiply any of the expressions by the rotation matrix 

£ to express it in terms of the global coordinate frame; 

(e) form the G- and H-functions for the manipulator including the G- 
and H-functions for the mass center of each link, using Table 3-3 
and Table 3-4; 

(f) form .***] and «p<p] ; 

(g) substitute into equation 3-68 for the dynamic equation. 

The simulation form of the dynamic equation can easily be obtained by 
rewriting equation 3-65 into the following form 
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(3-69) 

Integration of equation 3-69 will give the time history of the joint parameters 
resulting from the application of the generalized input loads. This concludes the 
discussion on the development of the kinematic and dynamic model of serial 
manipulators using the method of KIC. For more detail, readers are referred to 
Freeman and Tesar [14]. The next chapter will expand the modeling technique to 
include parallel manipulators via the transfer of generalized coordinates. 


CHAPTER 4 


TRANSFER OF GENERALIZED COORDINATES 

The discussion thus far has concentrated on finding the kinematic and 
dynamic models of serial manipulators in terms of the relative joint parameters. 
However, these may not necessarily be the models that one is interested in. For 
design and control purposes, one maybe interested in having the system model 
referenced to a different set of generalized coordinates. In the case of parallel 
mechanisms, one may desire to have the system model referenced to a common set 
of generalized coordinates. Unfortunately, at times, this model maybe impractical, 
if not impossible, to obtain directly. But, as it is usually true, most mechanisms can 
be modeled in terms of at least one set of generalized coordinates directly. This 
leaves researchers with the alternative to first determine the easily, or at least 
practically possible, direct model, then find a way to arrive at the desired model via 
the direct model. 

Freeman and Tesar [14] proposed using an isomorphic transformation 
technique called the transfer of generalized coordinates to obtain the desired model 
from the direct models. The development of this technique is based almost entirely 
on the principle of virtual work. This transfer of generalized coordinates procedure 
gives the method of KIC more generality and potential. As a result of this 
development, the method of KIC can now be used to model redundant or 
overconstrained systems, dual or multi-arms robots, and above all, multi-loop 
parallel mechanisms which is the theme of this work. 

4-1 Kinematic Model Transfer 

The transfer of the kinematic model can be broken down into two 
parts. One part is called the direct kinematic model transfer which has been briefly 
discussed in Section 3-2. 1.4. And obviously, the other part is the indirect kinematic 
model transfer. Direct kinematic model transfer is defined as any transformation that 
involves only the interchanging of dependent and independent system parameters. 
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On the other hand, indirect kinematic model transfer involves the introduction of 
one or more intermediate coordinate sets before arriving at the desired model. 

4-1.1 Direct Kinematic Model Transfer 


Consider again the resulting forward kinematic model given in Section 
3-2. 1.3. The first- and second-order time derivatives of a set of general position 
parameters (n) are given as in equations 3-36 and 3-37, 


ii = [Gj]S+i 


(3-36) 

(3-37) 


For the purpose of showing that H need not always represent the kinematic state of 
the end-effector, instead of using a as defined in Chapter 3, & will be used to 
represent the dependent system parameter, and ^ will represent any independent set 
of generalized coordinates, in the initial model. Following this scheme, the initial 
kinematic model is 


and 



(4-1) 


(4-2) 


However, the desired kinematic model is to have a as the independent system 
parameter set, and & as the dependent system parameter set. This gives the form for 
the kinematic model as 


<£> = 


’<u 


and 


<B = 


a + a 


H 


q<U 


(4-3) 

(4-4) 
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Rearranging equations 4-1 and 4-2 also can give a form similar to equations 4-3 and 
4-4 



Substituting equation 4-5 into equation 4-6 for ^ gives 



(4-5) 

(4-6) 


(4-7) 


Utilizing the generalized dot product introduced in Chapter 3 gives 



a T [G:] T ([G:]".[Hi,])[G:i'a 


(4-8) 


Comparing equation 4-3 with equation 4-5, it is seen that 

o:]-[o:]" 


(4-9) 


Similarly, comparison of equation 4-4 and equation 4-7 gives 

[»:,]- -[g;]' t ([g;] \[h;.])[c:] 1 (4 . 10) 

Equations 4-9 and 4-10 show how G- and H-functions change when the dependent 
system parameter becomes the independent system parameter, and vice versa. 

Notice that this interchange is possible only if [^®] is invertable, which means that 
it is non-singular and that q must be a potential set of generalized coordinates. 

As mentioned in Chapter 2, at singularity configurations, serial 
manipulators lose one or more DOF while parallel manipulators lose control of one 
or more DOF. However, for the purpose of this discussion, it is always assumed 

that [g®] is non-singular. Also realize how the post super- and subscript help to 
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denote the dependent and independent system parameters while performing the 
transformation. They also serve as a checking device when writing the equations. 
Any error will show up as an irregularity in the form of the super- and subscripts. 

4-1.2 Indirect Kinematic Model Transfer 


The previous section discussed how to interchange the dependent and 
independent system parameters if it is difficult to derive the desired model directly. 
This section will discuss the situation when it is laborious, if not impossible, to 
arrive at the desired model by direct kinematic model transfer. The procedure 
involves an intermediate set of generalized coordinates, which are employed to 
facilitate the determination of the desired model, due to its direct relationship with 
the desired coordinate set. Hence, the title indirect kinematic model transfer. 

Let's assume that it is desired to have the kinematic model of 3 
referenced to a set of generalized coordinates d, but what is available directly is the 
dependent system parameters & referenced to an initial set of generalized coordinates 

R Thus, the initial kinematic model is obtained according to equations 4-1 and 4-2, 
as 


and 


d = [g’] (g 

a = [c:]^ + i T [H^]i 


(4-D 

(4-2) 


Also, it is assumed that the kinematic coefficients of the dependent system 
parameters ^ referenced to the initial coordinates ^ are directly available. This gives 


and 

a=[G:]i + i T [n: 9 ]i 


(4-11) 

(4-12) 


However, what is desired is to express the kinematic state of a in terms of i as 



and 


d = [Gd]d 

ii = [G^]d + d T [HL]d 


(4-13) 
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(4-14) 


Rewriting equation 4-1 gives 

ffi-[Gj] a 


(4-15) 


Substituting equation 4-15 into equation 4-11 for ^ gives 

d = [ G l][ G J] a 

Rearranging equation 4-16 gives 

a -[oJ] [oil'd 

Comparing equation 4-13 with equation 4-17 yields 

[a:]-[o:][o:]" 


(4-16) 


(4-17) 


(4-18) 


This concludes the indirect transfer of the first-order KIC(ie., the G-functions). 

Next, the transfer of the second-order KIC will be derived. Rewriting 
equation 4-12 as 



(4-19) 


Substituting equation 4-19 into equation 4-2 gives 


a = 




(4-20) 


Rearranging equation 4-20 and using the generalized dot product gives 
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(4-21) 


Substituting equation 4-11 for $ into equation 4-21 gives 

a - [g d T [°i]’ T ( [h;.]-[g;]''. [hj.] )[o;]''i 


(4-22) 


Comparing equation 4-14 with equation 4-22 yields 

’hL] = [gJ]' t ( [hJ,] - [gJ] [h;,] )[gJ]" (4 . 23) 

as the desired second-order KIC. Equation 4-18 and 4-23 give the necessary G- 
and H-functions for the kinematic model with the dependent system parameter 
referenced to the desired set of generalized coordinates d. This procedure is not 
limited to the transformation of generalized coordinates involving only one 
intermediate set of generalized coordinates. There can be as many sets of 
intermediate generalized coordinates as the situation requires, provided that all the 
G- and H-functions are interrelated and can readily be determined either directly or 
indirectly. 


4-2 Dynamic Model Transfer 


Similar to the kinematic model transfer discussed previously, the 
readily available dynamic model referenced to an initial set of generalized 
coordinates given as 


r T - 


(4-24) 


where [***] and are as defined in equations 3-66 and 3-67, respectively, may 

not be the desired model. Actually, the dynamic model referenced to another set of 
generalized coordinates d is the ultimate goal. However, it maybe very difficult to 



obtain this model directly. Hence, an operation to transfer the dynamic model from 
the initial set of generalized coordinates to the desired coordinates set is necessary. 

It is assumed that the kinematic influence coefficients between the 
initial and the desired coordinates sets are available in the following form 


d — [oJ]i 

and 

s1 = |g5]£+<e 


(4-25) 

(4-26) 


Using the principle of virtual work, the dynamic equation can be expressed in terms 
of the desired input set ^ as 



(4-27) 

(4-28) 


Substituting equation 4-6, with q replaced by d, for 2 into equation 4-28, 
rearranging, and using the generalized dot product gives 



+ i T { [g;]' t . [p;„]-([g:J t [i;.][g;]'). [h;,] 


(4-29) 


. . T 

Substituting equation 4-5, with q replaced by d, for ^ and ^ into equation 4-29 
yields 
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l-[oJ] K][o:]'a 

-d T [G;] T {([c:] T .[p;„]) 

-([o a .] T [4.][cJ] ').[ H i.]} [o 4 .] 


(4-30) 


Comparing the form of equation 4-30 with that of equation 4-24, the dynamic 
equation referenced to the desired set of generalized coordinates (4) can be 


where 


and 


r • 1 .. . T r • 1 . l 


Id=[iddJsi + ji [Pdddjii-Td 

(4-31) 

r • l r dV T r • ir dr 1 


[idd] = [g<p] [i,J[gJ 

(4-32) 

[Pddd] = [G v ] ( [g„] .[p,,,]-[idd] •[h, <p 

}[ G <p] (4-33) 

i=[c:] T ii 

(4-34) 


Note that utilizing this generalized transfer, any readily obtained dynamic model can 
be transferred to any desired set of generalized coordinates once the KIC’s relating 
^ to ^ are obtained. 

For the purpose of discussion, the above mentioned procedure is not 

the only way to arrive at the model referenced to the desired set of generalized 

coordinates. An alternate procedure involves the derivation of intermediate dynamic 

models. Instead of finding the kinematic model relating the initial set to the desired 

set of generalized coordinates, then performing the dynamic model transfer as 

[ G <i] 

before, the dynamic model at each stage is determined. This means that if L u fJ, 
[h<p<p] ) M and [H dd ] are known, where 3, and ^ are as defined before, ■£<* is 
found in terms of which is in turn found in terms of—*. First, it is assumed that 



If is known. Second, transfer the dynamic model to the intermediate set of 
generalized coordinates as 



(4-35) 


Finally, transferring from the intermediate set to the desired set of generalized 
coordinates yields 



(4-36) 


as the desired dynamic model. In equations 4-35 and 4-36, procedures similar to 
those of equations 4-24 through 4-34 are implied. 

Both the procedures are valid as shown in Freeman and Tesar[14], 
The decision to choose either one of them is entirely up to the users. It depends on 
what variable relations are available and/or which is easier to obtain. However, the 
first approach is suggested by Freeman and Tesar[14] and Hudgens [21] for design 
and control purposes since it maybe necessary to investigate more than one set of 
generalized coordinates. 

4-3 Application of Transfer of Generalized Coordinates to Multi-loon 

Parallel Mechanisms 

Generally, the load capacity of serial manipulators is limited by the 
size of the actuators (Driga, Eppes and Flake [10]). Hence, for high load capacity, 
hydraulic actuators are commonly used due to their high load to weight ratio. 
However, the main shortcoming for using hydraulic actuators is low precision. As 
mentioned before, the error in serial manipulators is additive. If hydraulic actuators 
are used in serial manipulators, the inaccuracy will be even larger. Therefore, for 
any robot with high load capacity and acceptable precision, parallel structure robots 
are frequently used. 

Chapter 2 gave a brief discussion of the work on parallel mechanisms 
that has been done by other researchers. This section will introduce a different 
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approach to parallel mechanism modeling using the method of KIC along with the 
transfer of generalized coordinates. However, unlike the open-loop serial 
manipulators that have been discussed so far, parallel manipulator structures are not 
as straight forward as the serial manipulators due to the fact that there is more than 
one way to form a parallel manipulator. The discussion here will concentrate on 
mechanisms that can be classified as "fully parallel," such as the generalized 
Stewart Platform shown in Fig. 4-1. Although the figure shows six legs connected 
to a common platform with each of the legs having six DOF, the procedure which 
will be discussed shortly is equally applicable to mechanisms having any number of 
legs, provided that each of the link has the same number of DOF as does the 
operational space. 

The generalized Stewart Platform in Fig. 4-1 has six DOF at the 
platform, but it has thirty-six joints. To control this mechanism, one has to decide 
which of the six out of the thirty-six potential input locations to choose. For a start, 
examining the mechanism a little closer shows that it actually consists of six serial 
mechanisms connected to a common base and a common platform. Similar to the 
procedure discussed previously for serial manipulators, the First step is to obtain a 
' model directly for each leg with respect to a set of generalized coordinates that can 
easily be determined. By treating the other five legs as if they do not exist, each leg 

is modeled with respect to its own joint coordinate set r ^, where r(r=l, 2, ..., 6) is 
the leg number. This yields results similar to the serial manipulator discussed in the 
previous sections, ie., 




where £ represents the intermediate set of generalized coordinates associated with 
the six parameters describing the motion of the platform. Next, apply the direct 
kinematic transfer of equations 4-9 and 4-10 to give 



(4-37) 


and 


Desired Generalized Coordinates 


i 

d = [ l«P|. 2<P, . 3<P, . 4«P,, J<Pl. 6<P, 

Initial Generalized Coordinates 

r i T 

r$ = r<P,» .?J. r<P 3 » r<p 4 > ,<Pj. r<P* ' r=l,2,...,6 


Fig. 4-1 Generalized Stewart Platform* 

Adapted from Freeman and Tesar [ 14] 
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. r H q ’ q ] = -[ r Gl] T ([ r Gp] l .[ r Hj v ])[ r Gj] ' 
From the transfer of the dynamic model as in Section 4-2, one has 
[ V ]-[ r q l' T f T* If r’l"' 

i qqJ“L rVJ vJ [ r A tp<pJ [ r^tp J 

[■p;«]-[.o:]' T {([,o:]' T .[.p;„]) 



(4-38) 

(4-39) 

(4-40) 

(4-41) 


Notice that until this point, nothing has been said about the platform 
itself. Actually, the model of the platform can be included in one of the legs in the 
above derivation. However, it is felt that separating the platform model from the 
legs gives a more explicit physical meaning in the final formulation. Now that all 
the legs are referenced to the same intermediate set of generalized coordinates, they 
can be combined together along with the inertia effect of the platform and any load 
applied to the platform. As a result, the dynamic model can be expressed as 

[*qq] = [*qq] + X [ *qq] 

r -’ (4-42) 

Pqqq] = [ ^qqq] + S [ ^qqq. 

r -> (4-43) 


and 
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r i=[ r G:] T jJ 
6 , ( V 

-s '1-4 

"i Im 


(4-44) 


6_c 


where £ and 21 are a set of external forces and moments applied to the platform, 
respectively. The terms, t I( n] and [ P iqq] are the platform effective inertia matrix 
and inertia power array, respectively, defined as 


[^q] - 


[ P qqq]k;;“ 


67 T 

M 0 0 Q 

0 M 0 Q 

0 0 M Q 


Q. o. a [n 67 

[ 0 ] [ 0 ] ■ 

[0] [ Pkqq] 


, k = 4, 5, 6 


(4-45) 


(4-46) 


where 
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and the first three planes of [ P< ws3 are 6 by 6 null matrices, and 
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(4-48) 
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(4-50) 


are the components in the last three planes. Equations 4-48 to 50 are all 3 by 3 
matrices. 

Up to this point the kinematic and dynamic model of the generalized 
Stewart Platform has been formed referenced to the common set of generalized 
coordinates 4. This common reference gives the flexibility to transfer the model to 
any six of the thirty-six potential joint inputs. One may desire to have a single leg 
provide control of all the six DOF or one may desire to control only one DOF from 
each leg. For the sole purpose of this discussion, let's assume that it is desired to 
have the first joint of leg one, the second joint of leg two, and so on, provide 
control of the six DOF. This gives the desired generalized input set as 


d« 


li£l> 2^2 2 > 



(4-51) 


To obtain the First- and second-order KIC relating the desired set of generalized 
coordinates d to the intermediate set fl, simply extract the corresponding row of the 
G-functions and plane of the H-functions, from equations 4-37 and 4-38 for the 
respective legs. This extraction yields 
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and 
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Having formed the KIC required by the transfer equations, the desired dynamic 
equation can be written as 


Ia=[lL]ii + d T [pldd]d-li 

where 

[i; a ]=[c;]' T [i;,][a;f 

[p:J=[g:]' t {([o;]' t .[p;„]) 

and 


( 4 - 54 ) 

( 4 - 55 ) 

( 4 - 56 ) 

( 4 - 57 ) 


The above derivation shows how to model multi-loop parallel 
mechanisms in terms of the relatively simple serial manipulator model. The beauty 
of this procedure is that serial and parallel mechanisms can be treated in exactly the 
same way. All that is needed are the isomorphic transformation equations. 
Throughout the derivation, the desired equations are always in the same basic form, 
ie., time dependent terms are separated from the configuration dependent terms. 
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The G- and H-functions along with the L* J matrix and F J array also maintain the 
same form throughout, hence the term isomorphic. 

The procedure discussed in this section for multi-loop fully parallel 
mechanisms can be summarized in the following steps : 


(a) number all the legs and joint inputs; 

(b) determine the kinematic and dynamic model of every leg, in terms 
of the easily obtained set of joint coordinates, by treating each of 
them as a single serial linkage; 

(c) perform the generalized coordinate transformation on each leg from 
joint coordinates to a common set of generalized coordinates; 

(d) include the inertia effect of the platform and any applied load; 

(e) prepare to transfer the model to the desired reference by extracting 
the respective row from the G-functions, and plane from the 
H-functions; 

(f) form the desired dynamic equation by applying the transfer 
equations. 


In terms of symbols, the above procedure can be summarized and 


represented as 


[ r^ip] > [ ^ rlpip] » [ rPipqxp] > rl<p 

4 

i 

4 



This concludes the development of the kinematic and dynamic models 
of the generalized Stewart Platform using the method of KIC together with the 
isomorphic transformation of generalized coordinates. In the next chapter, all the 
techniques and procedures discussed so far will be used to model the Dynamic 
Docking Test System, which was described briefly in Chapter 2. 



CHAPTER 5 


KINEMATIC AND DYNAMIC MODELING OF THE 
DYNAMIC DOCKING TEST SYSTEM 

As discussed in Section 2-2, the Dynamic Docking Test 
System(DDTS) is a mechanism consisting of a base and a platform connected by 
six legs as shown in Fig. 2-5. All the legs are made up of prismatic joints with one 
end of the leg connected to the platform by a ball joint and the other end connected 
to the base by two intersecting revolute joints(or hooke joint). It is obvious that the 
structural arrangement of the DDTS is a variation of the generalized Stewart 
Platform addressed in Section 4-3. 

A conceptual view of the DDTS showing the relative position of all the 
connection points for the legs on the platform as well as the base is given in Fig. 
5-1. Notice that each leg has six DOF, three from the ball joint, two from the hooke 
joint and one from the prismatic joint. Although there can be one actuator for each 
DOF, the physical structure of the mechanism is designed such that only one 
actuator, located at the prismatic joints, is available for each leg. Therefore, the ball 
and hooke joints are free to rotate about their own axes. The following sections will 
discuss the development of the model, starting from coordinate frame definition to 
the final dynamic model. 

5-1 Kinematic Model 

The approach taken here is different from that of Hudgens [21]. The 
main difference is that they considered the three generalized coordinates for the ball 
joint, with the X-axis of the last joint pointing towards the center of the platform, 
the two generalized coordinates for the hooke joint, and the global reference 
coordinate(or the base reference coordinate) together as one complete set of 
generalized coordinates. These six coordinates make up their initial set On the other 
hand, the initial coordinates adopted in this work consist of three generalized 
coordinates, two located at the hooke joint and one at prismatic link, as shown in 
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Fig. 5-2. This requires a slightly different set of transfer equations to determine the 
platform based intermediate model, which will be discussed in Section 5-3. 

It is felt that separating the coordinates into small subsets will facilitate 
the derivation and subsequently the computation. As a matter of fact, in the initial 
model, instead of 6 by 6 matrices, only 3 by 3 matrices need to be dealt with. This 
is made possible by neglecting the three DOF of the ball joint, which will constitute 
three mutually orthogonal rotational axes. To neglect the ball joint in the first set of 
coordinates, the first-order KIC relating the hooke and prismatic joints to the 
platform must be independent of the three pseudo axes of the ball joint. This 
independency can be shown by employing the matrix partitioning technique to the 
G-functions as illustrated in Freeman [16]. However, the one shortcoming that 
arose from dividing into three sets of generalized coordinates is that it was 
necessary to perform several of the previously discussed generalized coordinate 
transfers. Since all the six legs are identical, only one of them need be discussed in 
the development of the total system model. 

5-1.1 System Definition 

As mentioned before, there are three DOF for each leg(neglecting the 
ball joint). Two of the freedoms are from the hooke joint, and the other one is from 
the prismatic joint. Following the convention adopted in Chapter 3, Section 3-2.1, 
the three coordinate frames for each leg are set up as shown in Fig. 5-2. The fixed 
frame for each leg is indicated as X, Y and Z in the figure. This frame is fixed 
relative to each leg so that it behaves like a local global frame for each individual 
leg. To transform the expression to the globally referenced frame at the center of the 
base, one needs only to multiply the local global frame results by a rotation matrix. 

Before all the different reference frames get too complicated, it is 
appropriate to define each of the terms that will be used throughout this work. From 
now on, leg frame refers to the fixed frame for each leg(or the local leg frame in the 
above paragraph), which is represented by the X, Y and Z orthogonal unit vectors. 
Base frame is referred to as the world coordinate frame and is located at the center 
of the base. X*, Y* and Z* are used to represent the three mutually orthogonal unit 




vectors for the base frame. Also, platform frame refers to the coordinate frame 
located at the center of the platform, represented by the lower case x, y and z, 
which will be discussed in a later section. The three coordinate frames mentioned 
above are shown in Fig. 5-3. 

Having a leg frame for each individual leg facilitates the modeling 
procedure. Since, with respect to this frame, all the six legs can be modeled 
identically. Referring back to Fig. 5-2, Lc is defined as the length of the piston 
cylinder, L r is the length of the piston rod, whereas, L 3 is the total length of the 
piston, measured from the origin of the leg frame to the end of the prismatic link. 
Notice that Lc and Lr are fixed length but L 3 is variable. 

Following the set up in Fig. 5-2 and the convention in Section 3-2.1, 
a simplified version of Fig. 5-2 can be made as illustrated in Fig. 5-4 along with the 
resulting link parameters. Defining 

£j = ( x j, Yj, Zj) ; j = 1 ,2,3 ( 5 „n 


and premultiplying the locally referenced vector a) Sj by the rotation matrix T, 


gives 


where 





Xj-i 

Yo-Di 

Z<j.i)X(j.i)j- 

X H Z (j-Dj 

Yj.t 


X(j-l) 
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Ce, S ej 
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-Ce, 
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0 


. ' Cfl ’ . 


S 9i = sin ( 0 j ) , C 9 .= cos ( 0;) 


(5-2) 


(5-3) 


Similarly, defining 
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3jit- ( X jk , Y jk , Zj k ) ; j = 1,2,3 ; k = j+l 
and premultiplying its local vector by the matrix T in equation 5-2 gives 


■c e ; 




C 01 C ei 

-Se, 

y §23 - 

Se,Ce 2 

> 2-34 - 

Se,Cej 

1 
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1 

00 

<x> 

Kl 

1 


. Sa > . 


(5-4) 


(5-5) 


Notice that ^23 and ^34 are identical. This is expected since ^34 j s j n the last link 
and can be arbitrarily chosen to be parallel to ^23 . 


5-1.2 Specification of the Platform Position and Orientation 


To begin the kinematic analysis of the mechanism, it is assumed that 
the kinematic state of the end-effector(or the platform) is known. For any rigid 
body in space, its position can easily be specified by a vector pointing from the 
origin of the reference frame to a fixed point in the body. However, there are many 
ways to specify the orientation of the rigid body as discussed in Hudgens [21]. 
Euler angles, Cayley-Klein parameters and quaternions are some of the possible 
ways. 

The method employed in this work to specify the position of the 
platform is by specifying the vector pointing from the origin of the base frame to the 
center of the platform. The orientation of the platform is given by a pair of 
orthogonal unit vectors fixed at the center of the platform. One of these vectors is 
normal to the surface of the platform. The vector cross product of these two unit 
vectors gives the third vector to make up the three mutually orthogonal unit vectors, 
which define the platform frame. Fig. 5-5 illustrates the specification for the 
position and orientation of the platform. Vectors a and £ represent the two mutually 
orthogonal unit vectors used to specify the location of x and z axes, respectively, 
of the platform frame in the base frame. 

Once the position and orientation of the platform is known, the 
positions of all the six connecting points (ie., the ball joints), which are fixed with 


Y : 


Fig. 5-5 Specification of the Platfoi 
and Orientation 



respect to the platform frame, can now be transformed to the base frame by a simple 
rotation matrix T, 


8 1 


T = [ 2 S x 3 S 
and a linear translation vector 

U = [u x - iv u z *] T 


(5-6) 


(5-7) 


Note that equation 5-6 is in the basic form of equation 5-2. 

5-1.3 Reverse Kinematics of Each Leg 

The coordinate frames for each leg are set up in the previous section. 
Furthermore, vectors Sj and -i* for each joint are determined using equations 5-3 
and 5-5. However, as stated in Chapter 3 that in all practical purposes, the end- 
effector(or in this case the point P in Fig. 5-1) kinematic state is usually known 
instead of the state at each joint. Thus, this section will discuss the reverse 
kinematic (position only) of each link via the geometric approach. 

The geometry of each leg can be represented as in Fig. 5-6. The 
origin of the leg is always at the origin of the leg frame. The other end of the leg is 
located at point P( X p , Y p and Zf> ), the ball joint, which is determined by knowing 
the specified platform position and orientation and the location of the point P 
relative to the platform frame. As indicated in the figure, each leg can be 

characterized by two parameters( ie., and ). The length of the leg can be 

written as 

L»“ V X p : + Y, 1 + Z, 1 (5-8) 


Also, 


Vl =cos'‘(5l 


(5-9) 
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Fig. 5-6 Reverse Position Kinematic of the Leg 
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and 


\}/ 2 = sin 


iLjSiny, 


ft ft 

By geometry and the definition of 1 and 2 , it can be shown that 


and 


9, = 180 + y 2 

0 2 = 1 80 + j 


(5-10) 


(5-11) 

(5-12) 


Equations 5-11 and 5-12 provide the solutions for the unknowns in equations 5-3 
and 5-5. 

5-1.4 Initial First- and Second-order Kinematics of Each Leg 


From the given conditions, treating the relative joint parameters as the 
independent system parameters, all necessary G- and H-functions can be easily 
obtained directly. Recalling equations 3-24 to 3-34, the velocity and acceleration of 
the ball joint can be written as 


and 


rE = [ rG 9] rl2 rI2 [ rH«<p] r 12 
r® 2 > rLt) 


(5-13) 

(5-14) 

(5-15) 


where r(r=l,2,...,6) is the leg number. However, instead of the 6 by 6 matrix for 
the G-functions and 6 by 6 by 6 array for the H-functions as before, the G-function 
here is a 3 by 3 matrix, where the H-function is a 3 by 3 by 3 array, since each leg 
has three DOF. 


From Table 3-3, the first-order KIC for each leg can be expressed as 
shown in Table 5-1. Similarly, using Table 3-3 and the fact that 


r £ = L 3 


-Ce, 


(5-16) 


where equation 5-16 is expressed in terms of the leg frame and 

rRl = JI 2 = Q (5-17) 

the second-order KIC are shown in Table 5-2. 

This section also includes the first- and second-order KIC for the 
center of mass of each link to be used in the dynamic model. They are tabulated in 
Table 5-3 and Table 5-4, respectively. Notice that the second-order KIC for point P 
and the center of masses in Table 5-2 and Table 5-4 are all symmetric matrices. 

5-1.5 First- and Second-order Kinematics of the Platform 


Recalling Section 5-1.2, the positions of the six connecting points at 
the ball joints are fixed with respect to the platform frame. Define the location of the 
ball joints in the platform frame as 


(pi). 


W = 


(pl) W 

r TV x 

(pl, w 

r vv y 

(P V 

r TT x 


(5-18) 


where the pre-superscript (pi) denotes the platform reference frame. To rotate the 
vector in equation 5-18 to the base frame, simply premultiply equation 5-18 with 
the rotation matrix T, which is defined as 
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[*“]- 

[fit]- 

[ < G l 4 } = 

[rGj]- 


0 0 0 
0 0 0 
1 0 0 


0 Se, 0 

0 -C 9 , 0 

1 0 0 


0 S e , Ce,S 0j 

0 -Ce, Se,Se 3 

1 0 -c 9j 


•L 3 S 9 i S 9j L 3 Ce,Ce 3 
L3C e ,Se 3 L 3 S 9i Ce 3 
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-Ce, 


Table 5-1 First-order KIC for Each Link 
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[rHi 2 ], ;; = [ Q Q Q ] ; i- 1,2,3 
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Table 5-2 Second-order KIC for Each of the Links 
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Table 5-2 Second-order KIC for Each of the Links 

(cont.) 
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Q Q 



-yS 9 S ej yC 9 ,C 9 , 



0 
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0 



L S 9) S 9j 
L C 9] S 9j 
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L C 9) C 9j 
L S 9l C 9j 

ls 9j 


c 9i s 9j 

Se,S 9j 

-Ce 5 


where Lc is the length from the origin of the link frame to the center 
of mass of the piston cylinder. And 


with L3 defined as the total length of the prismatic link, and L* is the 
piston rod length. 


Table 5-3 First-order KIC for Each of the Center of 
Masses 
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Table 5- 
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Second-order KIC for Each of the Center of 
Masses 



90 



-L CqjS q 2 
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“Se l Se 2 


*L SqCq 2 
•L C 0i Se 2 
C 9l C 02 
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Table 5-4 Second-order KIC for Each of the Center of 

Masses (cont.) 
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Letting 

£ = SX3 

and substituting into equation 5-5 yields 
T = [ a fi g ] 



( 5 - 19 ) 


( 5 - 20 ) 


where the first column is the unit vector of the platform x-axis, the second column 
is the y-axis and the last column is the z-axis, all expressed in the base coordinates 
frame, implied by the superscript (*). Thus 



( 5 - 21 ) 


( 5 - 22 ) 


To determine the G- and H-functions for the ball joints in terms of 
platform coordinates (which will be necessary to determine the G- and H-functions 
relating the joint coordinates to the platform coordinates U), the velocity and 
acceleration of the point P at the ball joint of leg r are written as 


r E = lI + i!jx f W 
and 

r E = ii + £x r W + i! 2 x(fi2x r w) 


( 5 - 23 ) 

( 5 - 24 ) 


C % ©L 
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where U is the position vector of the origin of the platform frame expressed in the 
base frame, and ® and a are the angular velocity and acceleration of the platform, 
respectively. From equations 5-23 and 5-24, the first-order KIC are 
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(5-25) 

nts in the three planes of the H-function are zero except the following 



0 

,W Y 

r w z ' 




[ hL] 1; 

m ; d 

0 

- r W x 

0 






0 

0 

-rW x 



(5-26) 



' - r w Y 

,W X 

0 




[ H L] 2; 

m ; n = 

0 

0 

r W z 




' 


0 

0 

- r W Y 



(5-27) 
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(5-28) 

4, 5, 6. As a result, the velocity and acceleration can be written as 

’*-['■ 

Gu]ll 

T 





(5-29) 


G^Jil+ r P [ 

’h:„] 'e 




(5-30) 


in terms of the G- and H-functions. 

This concludes the kinematic modeling of the DDTS(or the generalized 

Stewart Platform) referenced to the initial sets of generalized coordinates(ie., 
and U). One very important point to note is that the derivation so far expresses the 
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G- and H-functions for P in terms of ® in the leg frame, whereas, the G- and H- 
functions for P are in terms of 12 in the base frame. 

5-2 Initial Dynamic Model of the Lees 

Similar to the kinematic modeling discussed in the previous section, 
the initial dynamic model also considers one of the legs and treats it as if it is an 
isolated three DOF serial manipulator. Assuming that there is only inertial loading, 
the procedures to arrive at the initial dynamic model follow the derivation of Section 
3-3.2. Referring to Fig. 5-2, the model here assumes that the hooke joint is a solid 
cylinder, the piston cylinder and the piston rod are considered slender rods. Due to 
the symmetric nature of each of the links in the leg, all the local inertia tensors are 
diagonal matrices. 

Substituting the appropriate terms into equation 3-55, the leg frame 
referenced inertia tensor for each of the links in Fig. 5-2 are given in Table 5-5. The 
symbol I stands for the mass moment of inertia of each links, where 


1*1 

is along 

3l2 ; 

I*i 

is along 

Si 

1*2 

is along 

223 

* 

1*2 

is along 

S 2 

1*3 

is along 

§34 

* 

1*3 

is along 

S 3 


Substituting the inertia matrices in Table 5-5 and the G- and H-functions determined 
in Section 5-1 into the effective inertia matrix equation 3-66 and the inertia power 
array equation 3-67 results in the model coefficients tabulated in Table 5-6 and 

Table 5-7, respectively. Finally, having determined . and l 

substitution into equation 3-66, gives the initial inertial load expression for the 
dynamic model of each leg as 

rl<p = [ + 12 [ rF<pq><p]l2 


(5-31) 
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Table 5-5 Inertia Tensors 



9 5 


A 0 0 

0 B 0 

0 0 M 34 


where 



•A = I t i + M 2 j| ~2 Sfljj + M 34 L Se 3 + I*2Se,+ I X 3 Sej 
B = I i2 + I y3 + M 23 (^) +M 34 L 2 
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L = 


l 3 - 
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Table 5-6 Effective Inertia Matrix 
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Table 5-7 Inertia Power Array 
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5-3 Transfer of Generalized Coordinates to Obtain the Dynamic Model 

Referenced to the Platform 


Section 5-2 developed the initial inertial load model of each leg directly 
in terms of the relative joint parameters Since it is assumed that there is no 
external load applied to the system, the controlling equation of motion in equation 
5-31 can be written as 


rl,= rl. 


(5-32) 


Before arriving at the desired model, which will be referenced to the actuators, it is 
necessary to transfer the dynamic model in equation 5-32 to an intermediate set of 
generalized coordinates, which in this case is the platform coordinate set II. Using 
the relations 


i-M"* 

£=['g:]' , 'e+e t [ 'h;] 't 

’e-Mu 


(5-33) 

(5-34) 

(5-35) 


and 

r £=[ r G u p ]ii+ii T [ r H p u ]ii 


(5-36) 


equation 5-32 can be expressed as 


,4,][ t g:] ‘[ r G p ]ii+ii T [ r p, uu ]ii 


(5-37) 


where 
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(5-38) 


Also, substituting equations 5-35 and 5-36 into equation 5-34 for 


r • 

l. and £• gives 



(5-39) 


the G- and H-functions for the joints referenced to the set of generalized coordinates 
associated with the platform can then be expressed as 


and 



(5-40) 

(5-41) 


Note, the kinematic influence coefficients relating the initial coordinate sets to the 
platform coordinates 12, which are required to determine the platform based model, 
cannot be found by direct inversion of those relating 12 to ^ as in Hudgens [21], 
since they do not exist. Here, is related to P and P is related to 12 yielding the 
required relations of to 12 as given by the above equations. Next, utilizing the 
relation 


rlu= . 


-T 


rl <p 


(5-42) 


the 6 by 6 effective inertia matrix and the 6 by 6 by 6 inertia power array referenced 
to the platform set of generalized coordinates can then be expressed as 



(5-43) 
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[ ’C] - [ 'Gift ’c:] T [ '[ 'o’l 

[>;..] =['g!] 
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[ , o:]-[.g;][ , o:] 


(5-44) 

(5-45) 


Now that the dynamic model for each leg is referenced to the common 
platform generalized coordinates, they can be combined by substituting into the 
following equations similar to equations 4-42 and 4-43. 

cHM+ih;.] 

r-i (5-46) 

[p;,H p ..]+z[ rp ;j 

>-■ (5-47) 


Finally, to complete the model of the DDTS, the platform effective inertia matrix 
and its inertia power array need to be determined and substituted into the above 


equations. They can be expressed as 
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(5-48) 

; k , m , n = 4, 5, 6 
(5-49) 


where M p i is the platform mass and the inertia tensor is 
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with the local inertia tensor being 
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(5-50) 


(5-51) 


where W is the radius of the platform, which is the magnitude of the vector in 
equation 5-21. Note that the first three planes of ^* uuu ] are 6 by 6 null matrices, and 
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(5-52) 


(5-53) 

(5-54) 


where m, n = 4,5,6. 

As a result of the above transfer of generalized coordinates, the 
complete dynamic model of the DDTS is now referenced to the common set of 
platform coordinates. The next section will discuss the final transfer of system 
dependence to the desired set of generalized coordinates. 
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Transfer of the Dynamic Model to the Desired Set of Generalized 
Coordinates--The Input Actuators 


1 


Having the complete model of the system referenced to the platform 
coordinates, the final step is to transfer the model to each of the actuators. Since the 
actuator in each leg is at the third joint, the desired set of first-order KIC can be 
obtained by merely extracting the third row from the result of equation 5-40 for 
each of the legs, ie., 




(5-55) 


Similarly, the second-order KIC is obtained by extracting the third plane from the 
result of equation 5-41 for each of the legs, ie.. 
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(5-56) 


This extraction procedure has been described in Section 4-3. Once equations 5-55 
and 5-56 are determined, the effective inertia matrix and inertia power array can 
easily be computed by recalling equations 4-55 and 4-56 as 
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[i-]-[o3 T W[of 


and 


[p«a] = [ g;] t { ([g:] t . [puuu])-([iL] . [hi b ] 



(5-57) 

Gl]" 

(5-58) 


Finally, substituting equations 5-57 and 5-58 into the dynamic equation 

Id = ^ddj d. + d ^PdddJ d 


(5-59) 


gives the necessary generalized force at each of the actuators sL where 


d = 


1 S 3 

2 S 3 

3S3 

4 S 3 

5 S 3 

6^3 


(5-60) 

This concludes the dynamic modeling of the generalized Stewart 
Platform(or the DDTS) needed for the simulations addressed in the next chapter. 
Again, note that the final model essentially results from the multiple application of 
the isomorphic transformation equations to simple open-chain models. This avoids 
the much more difficult task of determining the desired dynamic model directly in 
terms of the desired generalized input coordinates. 



CHAPTER 6 


APPLICATION OF THE MODEL 

6-1 Verification of the Model 

The complete model of the generalized Stewart Platform(or the DDTS) 
was developed in Chapter 5. The final model is used to calculate the generalized 
forces for each input, given the position, velocity and acceleration of the inputs, 
which are determined once the platform motion is specified. This section will 
discuss three different approaches employed to verify the modeling technique, 
particularly the distribution of the system mass parameters introduced in Chapter 5. 
Additional verification of the general modeling technique can be found in Freeman 
and Tesar [14]. 

6-1.1 Special Case Model 

The first verification procedure involves specifying a system in which 
the desired model coefficients are available by inspection. Applying the transfer 
procedure and comparing those results with the known coefficient values completes 
the verification process. 

Instead of having the third(or the prismatic) joint from each leg be the 
input locations as in Section 5-4, the first joint, which is the fixed revolute of the 
hooke joint, is specified as the desired input location for each leg. Thus, 

T 

d = [ l<Pl 2<Pl 3 CPl 4<P: 5<Pl 6«Pl ] (6 _ x) 

is the desired input set Furthermore, only the first joint from each leg is given mass 
so as to have direct knowledge of the desired model. Hence, substituting r ^ 12 = 
10.0kg and the radius of the hooke joint as r = 0.06m into Table 5-6, gives the 
effective inertia matrix as 
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where 


r * l 

0.018 

0 

0 

[ rl<pq> J = 

0 

0 

0 

0 

0 

0 


A = In = = 0.018 kg.m 2 


( 6 - 2 ) 


(6-3) 


Note that the pre-subscript r in r ^ 12 refers to the leg number and the r in r 2 refers 
to the radius of the hooke joint. 

Following these assumptions, by inspection, the effective inertia 
matrix referenced to the desired set of coordinates(equation 6-1) is 

0.018 0 0 0 0 0 

0 0.018 0 0 0 0 

0 0 0.018 0 0 0 

0 0 0 0.018 0 0 

0 0 0 0 0.018 0 

0 0 0 0 0 0.018 



Also, the inertia power array L ^ ddd J is a 6 by 6 by 6 null array(ie., each actuator is 
only responsible for its own mass). 

Having knowledge of the expected results fort ^ dd J and I. ^ ddd -, the 
modeling procedure outlined in Chapter 5 is then applied. First, the directly 
available joint referenced model coefficients for each leg, 

£ > £ rH??] > £ > T = 1 , 2 , ...,6 ( 6 - 5 ) 


are determined. Second, applying the transfer of generalized coordinates to the 
results in equation 6-3 to obtain the model referenced to the common set of platform 
coordinates (equations 5-40 to 5-44) yields 

[ r Gl],[ r Hl u ],[ r i;J,[ r P; uu ] ; r = 1,2, ...,6 


( 6 - 6 ) 
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Since the platform is assumed to be massless, equations 5-46 and 5-47 become 

[£]- tVQ 
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(6-7) 


and 
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( 6 - 8 ) 


Equation 6-7 gives 
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0.0011 -0.0012 

0 

0 

0.0029 

0 

0.0012 0.0011 

-0.0023 

0 

0 

0.0030 

0 

0 

0 

0.0011 

0.0012 

0 

0.0039 

0 

0 

-0.0012 

0.0011 

0 

0 

0.0039 


(6-9) 


which is a symmetric matrix. Then, performing the extraction procedure discussed 
in Section 4-3 to the first- and second-order KIC in equation 6-4 to obtain 




( 6 - 10 ) 


and 
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( 6 - 11 ) 


as the G- and H-functions relating the desired input coordinates^) to the common 
platform coordinatesQi). Finally, applying the transfer equations again one arrives 
at the desired model coefficients(equations 5-57 and 5-58), 


[i«]-[g;] t [ i;„][g:]" 
;p;„]=[gi] t {([g:] t .[p:.]) • 

-([i-]*[h:])}[q:]" 


Substituting equation 6-9 and the result from equation 6-8 into equations 6-12 and 
6-13 yield 


and 



0.018 0 0 0 0 0 

0 0.018 0 0 0 0 

0 0 0.018 0 0 0 

0 0 0 0.018 0 0 

0 0 0 0 0.018 0 

0 0 0 0 0 0.018 


>«]*-[ 0 ] : k = l , 2,...,6 


(6-14) 

(6-15) 


where each plane in equation 6-15 is a 6 by 6 matrix. Comparison of the results of 
equations 6-14 and 6-15 with the known results partially verifies the model 
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developed in Chapter 5 and the computer program written for the simulation in the 
later part of this chapter. 

6-1.2 Verification of the Result for the First-order KIC 


One quick and simple way to check part of the model is by verifying 
the first-order KIC. From Table 3-3, the directly obtained first-order KIC can be 
expressed as 

G<p] = Si*( E-Ei) Sr* ( E-R2) S3 
= [ as 2 b( S 2 x s 3 ) S 3 

(6-16) 

where a and b are some constants, and from equation 5-25 

0 -0 0 r W t - r W Y ' 

1 0 - r W z 0 r W x 

0 1 r w Y - r w x 0 

J (6-17) 



are the platform referenced G-functions. Using the transfer of generalized 

f 'G 9 ! 

coordinates and realizing that only the third column of l u -I is needed since each 

leg's actuator is located at the third joint(prismatic), equation 5-45 can be written as 


'g:], ; .[ r c;]4 'of 


(6-18) 


Furthermore, since 

[ .o;]-[ 'o;j 


(6-19) 


the third row in the above expression can be shown to be 
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Rewriting equation 6-18 gives 




Substituting equation 6-20 into equation 6-21 yields 



where 


,W = 


rW x 

r W Y 

rW 2 


( 6 - 20 ) 


( 6 - 21 ) 


( 6 - 22 ) 


(6-23) 


which is as previously determined in equation 5-21. 

Since equation 6-18 relates the directly obtainable initial G-functions 
to the G-functions needed for the final transfer to the desired model, the above 
procedure effectively checks the validity of the results for the G-functions used in 
the desired model. 


6-1.3 Actuator Motion V erification 

Another way to partially verify the model is by checking the first- and 
second-order KIC used throughout the process. The method adopted here is to 
compare the result determined via the model developed in Chapter 5 with that 
obtained from direct mathematical calculation. This method also serves as a check 
for the verification process described in Section 6-1.2. 

Recalling equation 5-59, the terms d and d are the velocity and 
acceleration of the actuators (ie., the prismatic links) in this model. As seen 
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throughout the development in Chapter 5, the general expression for the velocity 
and acceleration of the linear actuators can be written as 

d»[ Gu]lL 

d = [Gu]il + li T [ H u d u ]li 


(6-24) 

(6-25) 


However, the velocity and acceleration can also be determined directly by taking the 
time derivative of equation 5-8, ie., 


l 3 2 =x p 2 +y p 2 +z p 2 


(6-26) 


to obtain the velocity expression as 

j X p X p +Y p Y p +Z P Z P 
— ; 


(6-27) 


Taking the time derivative of equation 6-27, gives 


t . X p X p + X p 2 +Y p Y p + Yp+ZpZp+Z,, 2 - L* 

L, 


(6-28) 


for the acceleration. 

Assuming that the platform motion ( II, II and II ) is known, 
comparing the results calculated from equation 6-24 with equation 6-27, and 
equation 6-25 with equation 6-28 for each leg, that is, 


[d]r;l= rU 
and 


(6-29) 

[d]r;P ri-3 


(6-30) 

where r=l, 2, ..., 6 is the leg number, verified the G- and H-; 

Functions derived for 

r -i r -i r 

d t r d t 

the model. This is because ^ U J and L ^ uu J are derived using 

l°:ii 

Hi, [ GuJ 

J j 

. ^ UU J and all the intermediate G- and H-functions. If . ^ u ] and [ ^ uu . 

are checked, 
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all the other G- and H-functions are checked too. This concludes the verification for 
the G- and H-functions. 

6-2 Simulation of the Dynamic Docking Test SvstemfDDTSl 

This section discusses several computer simulations for the DDTS 
based on the model established in Chapter 5. The simulations are coded in 
FORTRAN language employing a VAX/VMS 11/750 computer. A complete 
computer listing of the program written for one of the simulations is listed in 
Appendix B. The computer listing is for the case when the platform is translating 
and rotating about the X* axis using a class p=3, 3-4-5 polynomial curve, which 
will be discussed very shortly. To use the program for any other motion, one only 
needs to modify the subroutine "NEXT_MOTION" and the respective parameters in 
the calling statement. The input data corresponding to the subroutine 
"READ_DATA" for the program listed in Appendix B is listed in Appendix C. The 
main purpose of these simulations is to investigate the relative contribution to the 
overall generalized forces, required at each actuator, of the acceleration related (ie., 

I. ^ dd ] ) and the velocity related (ie., [ ^ ddd J ) terms. 

Four different motion specifications for the platform are adopted from 
Matthew and Tesar [29]. The first motion is the class p=2, constant acceleration 
curve as shown in Fig. 6-1. The subroutine "NEXT_MOTION" for this motion is 
listed in Appendix D. The second motion is the class p=3, 3-4-5 polynomial curve 
as in Fig. 6-2. The last two motions both belong to class p=4, one of them is the 4- 
5-6-7 polynomial curve and the other one is a 3 rd derivative trapezoidal curve, as 
illustrated in Fig. 6-3 and 6-4, respectively. Similarly, the subroutines 
corresponding to these two motions are listed in Appendix E and F, respectively. 
The use of class p=2, 3, or 4 in this context indicates that a jump or discontinuity 
occurs in the p th derivative of the motion function. The expressions for the 
displacement, velocity and acceleration corresponding to each of the motion curves 
are also listed along with the figures. The maximum displacement is denoted by y 
and the total time taken is denoted by Note that the 3 rd derivative 
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Fig. 6-1 Class p=2, Constant Acceleration 
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Fig. 6-4 Class p=4, 3 rd Derivative Trapezoidal 
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trapezoidal curve involves three constants Al, A2 and A3. They can be determined 
by using the expression 


Ai 

A 2 

A3 



2y'-2C, 

6 y - 6 ( Cit+ C 2 ) 

24y - 24 1 1 2 + C 2 t + C 3 | 


(6-31) 


and 



where Ci, C 2 , C 3 are the integration constants resulting from the initial conditions. 
Detailed derivation of equations 6-31 and 6-32 is given in Matthew and Tesar [29]. 
The subroutine used in the simulation to determine the constants Al, A2 and A3 is 
listed in Appendix G. 

Besides employing different classes of motion specifications, two 
arbitrarily chosen platform trajectories are investigated. First, the platform moves 
vertically along the X* axis from 3m to 4m, while simultaneously rotating about the 
same axis from 0 to -60 degrees, as shown in Fig. 6-5. In the second trajectory, the 
platform moves horizontally along the Z* axis from 0m to lm while X* remains 
constant at 3m. As before, the platform rotates about the X* axis from 0 to -60 
degrees. This trajectory is illustrated in Fig. 6 - 6 . For the second trajectory, only 
the class p=3, 3-4-5 polynomial motion is investigated. In addition, a different 
investigation is also conducted to study the relative contribution of the platform and 
leg mass to the effective inertia matrix and the inertia power array. This can be 
achieved by first assuming that only the platform has mass while the legs are 
massless, then assuming that the six legs have mass while the platform is massless. 
This investigation is conducted using the trajectory in Fig. 6-6 and the 
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class p=3, 3-4-5 polynomial, since it is felt that this trajectory provides a more 
general study due to its non-symmetric nature. The total time taken for each motion 
is 2sec, with an interval of O.lsec for every time step. The dimensions of the base, 
platform, legs and all other components used in the simulations are listed in Table 
6-1. Note that the numbers adopted here are all estimated values based on a 
photograph of the DDTS since the actual numbers were unavailable. The mass of 
the six legs and the platform are intentionally slightly overestimated to accommodate 
the likely application of the mechanism for large space vehicle docking purposes. 

The resulting actuator motions ( ie., d. and d ) and the required 
generalized forces at each actuator, along with the contribution from the velocity 
and the acceleration related terms, are plotted in Fig. 6-7 through Fig. 6-29. The 
notation used to denote the figures is a set of five alphanumerics. The code starts 
with an uppercase letter denoting which platform trajectory is being employed. "D" 
is for the trajectory that translates and rotates about the X* axis. "E" refers to the 
trajectory that translates along the Z* axis and rotates about the X* axis. The 
following digit indicates which type of motion specification is used. The digit "4" is 
for the class p=2, constant acceleration, "5" is for the class p=3, 3-4-5 polynomial, 
"6" is the class p=4,’4-5-6-7 polynomial, and finally, "7" refers to the class p=4, 
3 rd derivative trapezoidal curve. The last digit denotes the total time allowed, which 
in this work is always 2 for two seconds. The last lowercase letter refers to the leg 
number, from ”a" for leg 1 to "f” for leg 6. The two zeros in the code serve no 
practical purpose. For the first trajectory (ie., translating and rotating along/about 
the X* axis), the required generalized force curves of each leg are plotted following 
the plot for the position, velocity and acceleration of the actuator for that particular 
leg. 

6-3 Conclusions 

Comparing the plots for the platform motion ( ie., ii, and H ) with 
the plots for each actuator's motion (ie., ^ and £ ) does not show a direct 

relationship between them. This implies that in equations 6-24 and 6-25, 



Radius of the base = B = 3 m 


Radius of the platform = R = 2 m 

Radius of the Hooke joint = r = 0.06 m 
(modeled as cylinder) 

Length of the piston cylinder = L< = 3 m 

Length of the piston rod = Lf = 3 m 

Mass of the Hooke joint = M 12 = 10.0 kg 

Mass of the piston cylinder = M 23 = 200 kg 

Mass of the piston rod = M 34 = 175 kg 

Mass of the platform = M p i= 2250 kg 


Table 6-1 Estimated Dimensions and Mass of the 
Generalized Stewart Platform 
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Fig. 6-8 Actuator Forces D4002a 






Fig. 6-10 Actuator Forces D4002b 






Fig. 6-12 Actuator Forces D5002a 






Fig. 6-14 Actuator Forces D5002b 
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Fig. 6-16 Actuator Forces D6002a 
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Fig. 6-17 Actuator Motion D6002b 




Fig. 6-18 Actuator Forces D6002b 
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Fig. 6-20 Actuator Forces D7002a 
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Fig. 6-21 Actuator Motion D7002b 
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Fig. 6-22 Actuator Forces I)7002b 
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Fig. 6-24 Actuator Forces E5002a 
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(M) S30IOJ p3ZT|UI3U3Q 


Fig. 6-25 Actuator Forces E5002b 
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Fig. 6-26 Actuator Forces E5002c 



Fig. 6-27 Actuator Forces E5002d 
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Fig. 6-28 Actuator Forces E5002e 
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Fig. 6-29 Actuator Forces F5002f 
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d-[a:]n 

si = [ g„] ii + n T [ h^ u ] ii 


(6-24) 


(6-25) 


the G- and H-functions are nonlinear, as expected. This is particularly obvious by 
comparing Fig. 6-1 for the platform motion with Fig. 6-8 and 6-10 for the actuator 
motion. When the platform is moving with constant acceleration and deceleration, 
the actuators are moving with nonlinear acceleration and deceleration. Notice that 
for "D" trajectory, only leg 1 and leg 2(ie., "a" and "b”) are plotted. This is because 
this trajectory is symmetric about the X* axis. Hence, all the odd number legs are 
the same, as are all the even number legs. 

Next, looking at the plots for generalized forces versus time for each 
of the motions, as expected, at the beginning of the motions when the velocities are 
small, the acceleration related forces dominate the required generalized forces for 
the actuators. Overall, the acceleration related forces are generally more significant 
than the velocity related forces. This is because the elements of the effective inertia 

matrix * dd J generally have larger magnitude than that of the elements for the inertia 

power array . Pddd l Furthermore, it is observed from Fig. 6-7, 9, 11, ..., 21 that ^ 
remains relatively small, less than lm/s, with respect to d. Recalling equation 5-59, 


Id = Idd] il + d Pdddjd 


(5-59) 


for the generalized forces, since L Pddd J is operated on quadratically by 4 the 
velocity related term is expected to be smaller than the acceleration related term. In 

addition, examining the configuration dependent effective inertia matrix [ ^ dd l 
reveals that it is symmetric, which is consistent with its definition, and the 
magnitude of the diagonal elements are generally larger than the off-diagonal 

elements. This implies that [ Idd ] has the tendency to decouple, although not 
completely. An example of . * dd ] is shown in Table 6-2. Table 6-2 also shows an 
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1288.6 

334.8 

-114.2 

472.9 

-195.4 

-856.4 

334.8 

1622.9 

-919.5 

-516.3 

536.0 

-535.3 

-114.2 

-919.5 

1145.7 

370.3 

-52.4 

500.5 

472.9 

-516.3 

370.3 

1631.2 

-891.9 

-543.6 

-195.4 

536.0 

-52.4 

-891.9 

1226.9 

307.2 

-856.4 

-535.3 

500.5 

-543.6 

307.2 

1650.1 


Table 6-2 Examples of Effective Inertia Matrix and a 
Plane of Inertia Power Array for the 
Generalized Stewart Platform 


-172.0 

75.1 

20.9 

9.7 

- 73.7 

131.3 

25.0 

-423.6 

85.4 

-37.4 

53.3 

-8.1 

220.2 

58.7 

15.6 

1.6 

-152.2 

100.0 

-122.5 

-11.7 

77.5 

-120.4 

239.3 

-82.4 

-62.6 

-5.9 

32.9 

108.8 

-96.6 

-53.2 

-2.0 

18.3 

18.6 

24.8 

53.8 

-83.2 


I 
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example of one of the plane of the inertia power array 


[ P ddd]_ 


Reviewing the 


general form of the inertia power array indicates that it is not decoupled nor does it 


I«] 


matrix and 


P ddd] 


have the tendency to decouple. The characteristics of the 
array give some very important information concerning the controlling equation for 
the implementation of real-time feedforward control, which will be discussed 
shortly. 


This work provides all the essential information for two different 
control strategies (Fu, Gonzalez and Lee [17], Luh, Walker and Paul [27], Ogata 
[32], Whitney [48]) for the generalized Stewart Platform( or DDTS ). To employ 
pure, non-dynamic, feedback control for the DDTS, merely specify the desired 
platform motion, the program will calculate the kinematic state of the platform at 
each time step. Subsequently, the required actuator lengths corresponding to the 
respective time step are calculated. Hence, in each segment of time, the initial and 
final positions of the actuators are known. This gives the necessary information for 
any of a number of position control algorithms. 

The main objective of this work is, however, to provide the necessary 
software for future study using the dynamic model to develop a control scheme, 
possibly the computed torque technique, for the DDTS. Basically the computed 
torque technique can be slightly altered to become a layered control having both 
feedforward and non-linear feedback components. The interaction forces among all 
the various joints are compensated by the control components. The feedback 
component is used to compute the necessary correction torque to compensate for 
any deviation from the desired trajectory. Due to the computational demands of this 
control scheme, for any real-time control, it maybe necessary to simplify the 
controlling equations. It is customary although not always acceptable, to neglect the 
velocity-related coupling terms and the off-diagonal terms of the acceleration-related 
matrix for closed-loop control of serial manipulators. But, very little has been 
mentioned for parallel manipulators. Hence, this work provides some insights for 
the control of parallel manipulators. 

Investigating the [ * dd ] matrix and the [ ^ ddd ] array of the Stanford 
manipulator, generally shows that the inertia matrix is highly decoupled, the off- 
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diagonal elements are either zero or much smaller than the diagonal elements, and 
the elements in the t ^ ddd . array are mainly zero. Examples of the [ ^ dd ] matrix and 

the [ ^ ddd ] array for the Stanford manipulator are shown in Table 6-3. This 
observation tends to support the simplification scheme adopted for some serial 
manipulators as mentioned before. However, this scheme may not work effectively 

for parallel manipulators since the [ * dd ] matrix for parallel manipulators is not as 

f P * ] 

decoupled as it is for some serial manipulators and ddd J array is relatively 
significant in the parallel case. As a result, employing the same simplification 
scheme to the parallel manipulators is likely to result in larger error, subsequently 
relying more heavily on feedback control. 

The investigation conducted to study the effect of the platform mass 
relative to the leg mass in the effective inertia matrix and the inertia power array 

shows that with only the platform having mass, the resulting elements in the [ ^ di ] 
matrix generally have the same order of magnitude, although the main diagonal 
elements are slightly larger than the others. On the other hand, when the six legs 

have mass while the platform is massless, the main diagonal elements in the t * dd ] 
matrix are generally larger than the off-diagonal elements by an order of magnitude. 
These two cases indicate that the platform has a larger coupling effect than the legs 
in the final dynamic model. This provides information for one of the essential 
factors in the design of parallel manipulators. The information indicates that the 

[ * dd ] matrix has the tendency to decouple when the mass of the legs increases, for 
the purpose of increasing the rigidity, relative to the platform. Unfortunately, the 
inertia power array does not show any noticeable general tendency. Examples of the 

[ ^ dd ] matrix and L ^ ddd ] array are shown in Table 6-4 and Table 6-5 for the case 
when the legs are massless and when the platform is massless, respectively. 

It is not the intention of this work to recommend the control strategy to 
use for parallel manipulators. However, this work provides the essential model and 
software for further study of parallel manipulators. To arrive at an efficient and 
relatively accurate control scheme for real-time control of the generalized Stewart 
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26920.0 

0.0 

-570.0 

0.0 

0.0 

0.0 

0.0 

23045.0 

0.0 

0.0 

10.0 

0.0 

-570.0 

0.0 

95.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

15.0 

0.0 

0.0 

0.0 

10.0 

0.0 

0.0 

10.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 


0.0 

-5.0 

0.0 

0.0 

0.0 

0.0 

-15.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

-10.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 


Table 6-3 Examples of Effective Inertia Matrix and 
a Plane of Inertia Power Array for the 
Stanford Manipulator 
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781.9 

398.5 

-47.2 

412.4 

-364.2 

-514.4 

398.5 

866.5 

-573.8 

-249.4 

450.2 

-340.0 

-47.2 

-573.8 

784.6 

386.4 

-346.0 

484.2 

412.4 

-249.4 

386.4 

836.8 

-476.9 

-232.5 

-364.2 

450.2 

-346.0 

-476.9 

923.1 

330.3 

-514.4 

-340.0 

484.2 

-232.5 

330.3 

819.6 


35.5 

-75.4 

-89.8 

71.1 

134.8 

56.6 


2.4 

20.3 

158.6 

-45.8 

-76.1 

-46.1 

i 

-21.0 

85.2 

-69.7 

-50.6 

31.6 

-67.4 


28.0 

50.0 

-6.5 

-5.9 

-29.0 

51.4 


60.0 

-45.4 

-11.9 

46.2 

-176.1 

37.1 

:i 

28.2 

-16.8 

-68.3 

70.2 

20.1 

-220.9 

i! 

!: 


Table 6-4 Examples of Effective Inertia Matrix and 

a Plane of Inertia Power Array for the Case 
when the Platform has Mass but the Legs are 
Massless 


i 
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388.7 

31.8 

- 5.5 

55.2 

- 58.4 

- 165.3 

31.8 

460.2 

- 169.9 

- 39.8 

45.3 

- 86.8 

- 5.5 

- 169.9 

397.2 

61.5 

- 70.7 

42.3 

55.2 

- 39.8 

61.5 

397.4 

- 154.6 

- 41.4 

- 58.4 

45.3 

- 70.7 

- 154.6 

414.7 

46.8 

- 165.3 

- 86.8 

42.3 

- 41.4 

46.8 

444.5 


27.6 

- 22.2 

- 19.5 

- 13.0 

11.8 

9.1 

- 44.9 

62.2 

11.7 

19.4 

17.2 

- 15.0 

- 43.2 

13.0 

- 72.8 

25.6 

25.8 

- 31.5 

3.8 

- 3.4 

- 10.2 

- 27.7 

- 9.9 

20.6 

39.8 

- 0.5 

13.8 

- 11.1 

- 13.0 

1.9 

33.4 

- 1.5 

- 6.1 

2.3 

- 22.1 

3.5 


Table 6-5 Examples of Effective Inertia Matrix and 

a Plane of Inertia Power Array for the Case 
when the Legs have Mass But the Platform 
is Massless 
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Platform, additional research focusing on different trajectories and motion 
specifications needs to be done. 

6-4 Summary 

In the second chapter, a general overview of the parallel manipulators 
classified as generalized Stewart Platforms was presented. The basic design concept 
and the control of the mechanism originated by Mr. D. Stewart was discussed 
briefly. Subsequently, various applications that stemmed from the original concept 
were discussed along with the different approaches taken by various researchers to 
analyze the mechanism. 

The modeling technique adopted in this work, called the method of 
Kinematic Influence Coefficients, was addressed in Chapter 3. The notational 
scheme employed to facilitate the derivation of the model was also introduced. The 
presentation focused on modeling open-loop kinematic chains(or serial 
manipulators). The discussion included forward and reverse kinematic and the 
development of the dynamic model of serial manipulators. 

Chapter 4 concentrated on the development of an isomorphic 
transformation technique called the transfer of generalized coordinates. This chapter 
started with the discussion of kinematic and dynamic model transfer for serial 
manipulators and then further extended the technique to include multi-loop parallel 
mechanisms. A general procedure was developed to perform the transfer of system 
dependence from any initial set of coordinates to the desired set of generalized 
coordinates. 

Utilizing the modeling technique developed in the previous chapters, 
Chapter 5 established a complete model of the Dynamic Docking Test System. 
Starting from initializing the Denavit-Hartenberg parameters to the final desired 
dynamic model referenced to the common platform coordinates set, through a 
number of intermediate sets of generalized coordinates, this chapter described the 
procedures used to arrive at the final model for the computer simulation. 

As a note of advice for similar simulation programs in the future, it is 
felt that FORTRAN language is inefficient and cumbersome for the modeling 
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technique adopted in this work due to the many matrices and higher dimensional 
arrays involved in the operation. A more efficient computer language suitable for 
matrix operation (eg., APL) will reduce the time and effort spent in coding and 
debugging the program. However, APL may not be computationally efficient for 
real-time control. 


APPENDIX A 


DEVELOPMENT AND DEFINITION OF GENERALIZED 
SCALAR ( • ) PRODUCT OPERATOR FUNDAMENTAL TO 
DYNAMIC MODELING AND TRANSFER OF COORDINATES 

1. Quadratic operation of a MATRIX on a three dimensional array. 

Given: 

[ A ] = M x N Matrix 

[ B ] = N x M x M Array 

Define: 

= N x N x N Array 


2. Quadratic operation of a VECTOR on a three dimensional array. 
Given: 

d = M component column vector 


[A] T [B][A] 


[A] T [B] I;; [A] 

[A] T [B] 2;; [A] 

[A] T [B] N;; [A] 


Define: 


d T [B 


/d T [B] 1;; d\ 

d T [B] 2;; d 


= N x 1 vector 


\d T [B ] N;: d/ 
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3 . Vector multiplication of quadratic result 
Given: 

[ C ] = K x N matrix 

[ C ]k; = row of [ C ] 


Then 

[C] k; (d T [B]d) = [C] lc;I d T [B] 1;;£ i + [C] k; 2 d T [B] 2;; £i 

+ . . . + [C] k;N d [ B ] N;; li 
= £l T [C] k; 1 [B] 1;;S i + d T [C] lc ., 2 [B] 2;; d 

+ ..•+4 [ C 3 |c;N [ B ]>J;; d 

= d T | X [ c ] k;n [ B ] o;; 



= scalar 


Define operator ( •) "DOT' 

N 

[ c ] k; •[ B ]s£[ C ] k;a [ B = Mx M matrix 
0 - 1 

= scalar multiplication of planes followed by a 
summation of the resulting planes 

4 . Matrix multiplication of quadratic result using ( • ) operator 

d T ( [ C ]l; •[ B ] ]d 

\si T ( t c ] ti -c b ] )<iy 


[ C ] ( d T [ B J d ) = 
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= d T { [ C ] .[ B ] )sl 
= K x 1 vector 

where ([C].[B]) = KxMxM array 



APPENDIX B 


SIMULATION PROGRAM LISTING 


155 


156 


PROGRAM STEW ART_PLATFORM 
C ALL UNITS ARE IN SI 
C Y AND Z POSITIONS FIXED 

C X MOVED FROM Xi(m) TO Xf(m) AND ORIENTATION FROM Ai 
C TO Af (DEGREES) 

C THE NOTATIONS USED IN THE PROGRAM MOSTLY 
C CORRESPOND TO THE DERIVATION IN THE THESIS, 

C EXCEPT THE FINAL DYNAMIC MODEL WHERE INSTEAD OF 
C THE SUBSCRIPT "d" AS IN THE THESIS, THIS PROGRAM 

C USED "q” 

REAL L3,L,II(3,3),Izzl,Izz2,Ixx2,Ixx3,Iyy3,M23,M34,Lc,Lr 
DIMENSION P_PL(6,3),P_GL(6,3),P_LK(6,3), 

+ ZSIphi_LK(3,3),ZPphi_LK(3,3,3),PF_Iuu(6,6), 

+ PF_Puuu(6,6,6),ZSIphi(3,3),ZPphi(3,3,3), 

+ ZGp_LK(3,3),ZHppJLK(3,3,3),ZGp_phi(3,3), 

+ ZHp_phi(3,3,3),PHI3( 1 00,6),Q(6, 1 ),UDOT(6, 1), 

+ UDDOT(6, 1 ),QDOT(6, 1 ),QDDOT(6, 1 ) 

COMMON/ORIEN/X 1 , Y 1 ,Z1 ,X 1 2, Y 1 2,Z 1 2 
COMMON/ARR_Gu/Gu(6,3,6) 

COMMON/ ARR_Huu/Huu(6, 3,6,6) 

COMMON/D ERECT/DGp(6,3,3)DHpp(6, 3,3, 3) 

COMMON/G_Hp_phi/Gp_phi(6,3,3)Hp_phi(6,3,3,3) 

COMMON/ARR_phi_uu/Gphi_u(6,3,6),Hphi_uu(6,3,6,6) 

COMMON/SINERTIA_phi/SIphi_LK(6,3,3) 

COMMON/POWER_phi/Pphi_LK(6,3,3,3) 

COMMON/SINERTIA_u/SIuu(6,6,6) 

COMMON/POWER_u/Puuu(6,6,6,6) 

COMMON/ARR_Gq_u/Gq_u(6,6) 

COMMON/ARR_Hq_uu/Hq_uu(6,6,6) 

COMMON/Iuu/TOT_Iuu(6,6) 

COMMON/Puuu/TOT_Puuu(6,6,6) 

COMMON/Iqq/ST AR_Iqq(6,6) 



COMMON/Pqqq/P_STAR_qqq(6,6,6) 

C0MM0N/CGpHpp/L3 

READ IN ALL THE NECESSARY DATA 

CALL READ_DATA(X1,Y1,Z1,X12,Y12,Z12,XU,YU,ZU, 

X 1 f, Y 1 f,Z 1 f,X 12f, Y 1 2f,Z 1 2f,XUf,YUf,ZUf, 

R,B ,Izz 1 ,Izz2,Ixx2,Ixx3 ,Iyy 3,M23,M34,Lc,Lr, 
TMAX,NSTEP, THETA, THETAf) 

ATTENTION: ANGLE THETAf IS SPECIFIED AS POSITIVE, 

BUT THE ROTATION CAN ONLY BE NEGATIVE 
BECAUSE OF THE ASSUMPTION THAT THE 
INITIAL ANGLE ALWAYS STARTS FROM 
0 DEGREE, WHICH IS WHEN THE al2 AXIS IS 
PARALLELED TO THE BASE Y* AXIS. 
THETAf=-THETAf*3. 1415926/1 80. 

XUi=XU 

THETAi=THETA*3. 14 15926/ 1 80. 

CALCULATE TIME STEP 
TSTEP=TMAX/NSTEP 
WRITE( 1 ,*)'TIME STEP=',TSTEP 
DO 111= 1 ,N STEP+ 1 
PTIME=(ni-l)*TSTEP 

CALCULATE THE NEXT PLATFORM POSITION AND 
ORIENTATION 

CALL NEXT_MOTION(TMAX,PTIME,XUi,XUF,THETAi, 

THETAf, XU, THETA, V,W,ACC,AL, 
Y12,Z12) 

WRITE(1,*)' ' 

WRITE(1,*)'PRESENT TIME=’,PTIME 

WRITE(1,55)XU,YU,ZU 

WRITE( 1 ,*)’VELOCITY OF PLATFORM=’,V 

WRITE( 1,*)' ACCELERATION OF PLATFORM=’,ACC 

WRITE( 1 ,66)X 1 , Y 1 ,Z 1 
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WRITE( 1 , 77)X 1 2, Y 1 2,Z 1 2 
WRrTE(l,*)THETA=', THETA 

C CALCULATE THE POSITION OF POINT P IN PLATFORM 

C FRAME 

CALL P_PLATFORM(R,P_PL) 

C GET THE ROTATION MATRIX TO ORIENTATE THE 

C PLATFORM TO THE BASE FRAME 

CALL TRANSF_P_G 

C TRANSFORM ALL THE POINTS ON THE PLATFORM TO THE 
C BASE FRAME 

DO 1=1,6 

CALL P_GLOB AL(I,Xu,Y u,Zu,P_PL r P_GL) 

ENDDO 
DO 1=1,6 

C TRANSFORM ALL THE POINTS ON THE PLATFORM TO 

C THEIR RESPECTIVE LEG FRAME 

CALL P_LINK (I,B ,P_GL,P_LK) 

C CALCULATE THE G- AND H-FUNCTIONS FOR THE 

C PLATFORM 

CALL CAL_Gu(I) 

CALL CAL_Huu(I) 

C CALCULATE THE G- AND H-FUNCTIONS FOR THE LEGS 

CALL CAL_G_Hp_phi(I,P_LK(1, 1 ),P_LK(I,2),P_LK(I,3), 

+ ZGp_phi,ZHp_phi) 

C DIRECT TRANSFER OF Hp_phi 

CALL CAL_Hphi_pp(I,ZGp_phi,ZHp_phi) 

ENDDO 

C TO DETERMINE Gphi_u AND Hphi_uu 
CALL C AL_G_Hphi_u u 

C SET UP THE INERTIA MATRICES AND POWER ARRAY 

DO 1=1,6 

. CALL CAL_INERTIA(I,Izzl,Izz2,Ixx2,Ixx3,Iyy3, 



159 


+ M23,M34,Lc,Lr,SIphi_LK) 

CALL CAL_POW_phi(I,Ixx2,Ixx3,Iyy3,M23,M34, 

+ Lc,Lr,Pphi_LK) 

ENDDO 

C TO DETERMINE Iuu AND Puuu 

CALL CAL_Iuu_Puuu 

C CALCULATE THE PLATFORM INERTIA MATRIX AND POWER 
C ARRAY 

CALL PFORM_INERTIA(R,II,PF_Iuu) 

CALL PFORM_INEPOW(n,PF_Puuu) 

C CALCULATE THE INERTIA MATRIX AND POWER ARRAY 

C FOR THE SYSTEM 

CALL I_ST AR_uu(PF_Iuu,TOT_Iuu) 

CALL P_STAR_uuu(PF_Puuu,TOT_Puuu) 

C DETERMINE THE FINAL DESIRED FIRST- AND SECOND- 
C ORDER KIC,Gq_u AND Hq_uu 

CALL CAL_Gq_u 
CALL CAL_Hq_uu 

C DETERMINE THE DESIRED INERTIA MATRIX AND POWER 
C ARRAY I*qq AND P*qqq 

CALL CAL_Iqq_Pqqq 

C SPECIFIED THE PLATFORM MOTION 

UDOT(3,l)=V 
UDOT(4,l)=W 
UDDOT(3,l)=ACC 
UDDOT(4,l)=AL 

C CALCULATE THE ACTUATORS MOTION 

CALL C AL_QDOT (UDOT,QDOT) 

CALL C ALQDDOT (UDOT,UDDOT,QDDOT) 

WRITE(1,88) 

DO K=l,6 

WRITE( 1 ,99) UDOT(K, 1 ),UDDOT(K, 1 ),QDOT(K, 1 ), 
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+ QDDOT(K,l) 

ENDDO 

C CALCULATE THE REQUIRED GENERALIZED FORCES FROM 

C EACH ACTUATOR 

CALL GEN_FORCES(QDOT,QDDOT) 

ENDDO 

55 FORMAT(X,'Xu=’,F15.10,5X,'Yu=',F15.10,5X,Zu=',F15. 10) 

66 FORMAT(X,'X1=',F15.10,5X,'Y1=',F15.10,5X,'Z1=',F15.10) 

77 FORMAT(X,'X12=',F15. 10,4X,'Y12=’,F15.10,4X,'Z12=', 

+ F15.10) 

88 FORMAT (/,6X,'UDOT’, 1 4X/UDDOT’, 1 3X, ’QDOT', 1 4X/QDDOT') 

99 FORMAT(X,4(F13.10,5X)) 

STOP 

END 

* * * * * * * * * * ** * * * * * * ** * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
* * 

* SUBROUTINE READ_DATA * 

* * 
****************************************************************** 

SUBROUTINE RE AD_D ATA(X 1 i, Y 1 i,Z 1 i,X 1 2i,Y 1 2i,Z 1 2i,XUi, 

YUi,ZUi,X 1 f , Y 1 f,Z 1 f ,X 1 2f , Y 1 2f , 
Z12f,XUf,YUf,ZUf,R,B,Izzl ,Izz2, 

Ixx2, Ixx3 ,Iyy 3 ,M23 ,M34,Lc,Lr, 
TMAX,NSTEP,THETAi,THETAf) 

REAL Izzl,Izz2,Ixx2,Ixx3,Iyy3,M23,M34,Lc,Lr 
C READ IN INITIAL ORIENTATION OF THE PLATFORM 

READ (40,*) Xli,Yli,Zli,Xl2i,Y12i,Z12i,THETAi 
C READ IN INITIAL POSITION OF THE PLATFORM 
READ (40,*) XUi,YUi,ZUi 

C READ IN FINAL ORIENTATION OF THE PLATFORM 
READ (40,*)X 1 f, Y 1 f,Zl f,X 1 2f,Y12f,Z 1 2f,THETAf 
C READ IN FINAL POSITION OF THE PLATFORM 
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READ (40,*) XUf,YUf,ZUf 
C READ IN PLATFORM AND BASE RADIUS 
READ (40,*) R,B 

C CALCULATE MASS MOMENT OF INERTIA 

CALL GET_CONST(Izzl,Izz2,Ixx2,Ixx3,Iyy3,M23,M34,Lc,Lr) 

C READ IN TOTAL TIME TAKEN 

READ (40,*) TMAX 

C READ IN NUMBER OF STEPS NEEDED 
READ (40,*) NSTEP 
RETURN 
END 

****************************************************************** 
* * 

* SUBROUTINE NEXT_MOTION * 

* * 
** * * * * ** * * * * * **** * ** * *** * **** * * ** * ******** * * * ** * * * * * * * * * ***** * * * * S|C 

SUBROUTINE NEXT_MOTION(TMAX,PTIME,XUi,XUf, 

+ THETAi,THETAf, XU, THETA, V, 

+ W,ACC, AL, Y 1 2,Z 1 2) 

C THIS SUBROUTINE CALCULATES THE NEXT PLATFORM 
C MOTION USING D5002 
X=XUf-XUi 
TTA=THETAf-THETAi 
T=PTTMETMAX 

XU=XUi+X*(10.*(T**3)-15.*(T**4)+6.*(T**5)) 

V=(X/TMAX)*(30.*(T**2)-60.*(T**3)+30.*(T**4)) 

ACC=(X/TMAX**2)*(60.*T-180.*(T**2)+120.*(T**3)) 

THETA=THETAi+TTA*(10.*(T**3)-15.*(T**4)+6.*(T**5)) 

W=(TTA/TMAX)*(30.*(T**2)-60.*(T**3)+30.*(T**4)) 

AL=(TTA/TMAX**2)*(60.*T-180.*(T**2)+I20.*(T**3)) 

Y12=COS(THETA) 

Z12=SIN(THETA) 
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THETA=THETA* 1 80./3. 1415926 

RETURN 

END 

****************************************************************** 
* * 

* SUBROUTINE P_PLATFORM * 

* * 
****************************************************************** 

SUBROUTINE P_PLATFORM(RE) 

C DETERMINE ALL THE POINTS ON THE PLATFORM IN TERMS 
C OF THE PLATFORM FRAME 

DIMENSION P(6,3) 

C ASSUME THAT THE ANGLE BETWEEN THE POINTS TOGETHER 
C IS 5 DEGREES 

BETA=5. 

P(1,1)«R 

P(2,l)=R*COSD(BETA) 

P(2,2)=R*SIND(BETA) 

P(3,1)=R*COSD(120.) 

P(3,2)=R*SIND(120.) 

P(4, 1 )=R*COSD( 1 20. +BETA) 

P(4,2)=R*SIND(120.+BETA) 

P(5,1)=R*COSD(240.) 

P(5 ,2) =R* SIND(240. ) 

P(6, 1 )=R*COSD(240.+BETA) 

P(6,2)=R*SIND(240.+BETA) 

RETURN 

END 



* * * ****** * **** * * a*e ******* * * * **** * ********** * **** * *** * ** * * * * * a|e *** »|c * * 


* * 

* SUBROUTINE P_GLOBAL * 

* * 
****************************************************************** 

SUBROUTINE P_GLOBAL(I,Xu,Yu,Zu,P_P,P_G) 

C THIS SUBROUTINE TRANSFORMS ALL THE POINTS ON THE 
C PLATFORM TO THE BASE FRAME 

COMMON/ARR_R/ARM(6,3) 

COMMON/TRAN_P_G/R(3,3) 

DIMENSION P_P(6,3),P_G(6,3),P(3,1),AR(3,1) 

P(1,1)=P_P(I,1) 

P(2,1)=P_P(I,2) 

P(3,1)=P_P(I,3) 

CALL MATRIX_MULT(3,3,R,3,1,P,AR) 

C XP,YP,ZP IN GLOBAL COOR 
ARM(I,1)=AR(1,1) 

ARM(I,2)= AR(2, 1 ) 

ARM(I,3)=AR(3,1) 

P_G(1, 1 )=Xu+ ARM(1, 1 ) 

P_G(I,2)=Y u+ARM(I,2) 

P_G(I,3)=Zu+ARM(I,3) 

RETURN 

END 

* ** * * 3|c * * * * * * * * ** * * * * * * ** * * * * * * * * * * * * * ** * * * * * * * * * * * * * * * * * * * * * * * * * * * 

* * 


* SUBROUTINE P_LINK 

* 


* 

* 


********************************************************* ********* 


SUBROUTINE P_LINK(I,B,P_G > P_L) 

DIMENSION P_G(6,3),P_L(6,3),P_LK(3, 1 ),P(3, 1 ),R(3,3), 
+ RI(3,3) 
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C XP,YP,ZP IN LEG COORDINATE FRAME 
C ASSUME THAT THE ANGLE BETWEEN THE TWO POINTS 

C TOGETHER IS 5 DEGREES 
DELTA=5. 

IF(I .EQ. 1) AL=0. 

EF(I .EQ. 2) AL=DELTA 
EF(I .EQ. 3) AL=120. 

IF(I .EQ. 4) AL=120.+DELTA 
IF(I .EQ. 5) AL=240. 

IFa -EQ. 6) AL=240.+DELTA 
CAL=COSD(AL) 

SAL=SIND(AL) 

C POINT 1 ON THE BASE IS CONNECTED TO POINT 2 ON THE 
C PLATFORM AND SO ON 

J=I+1 

EF(I .EQ. 6)J=1 
P(1,1)=P_G(J,1) 

P(2, 1 )=P_G(J,2)-B *C AL 

P(3,1)=P_G(J,3)-B*SAL 
CALL ROT_MATRIX(I,R) 

CALL MATRIX_INV(3 JUU) 

CALL MATRIX_MULT (3,3 ,RI,3 , 1 ,P,P_LK) 

DO K=l,3 

P_L(I,K)=P_LK(K,1) 

ENDDO 

RETURN 

END 



165 


* * * * s|c * * * * ** * ***** * ******* ******** * * * ********* * ****** ************* * 

* * 

* SUBROUTINE CAL_Gu * 

* * 
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 9|e * * * * * * * ** * * * * * * * * * * * * 

SUBROUTINE CAL_Gu(N) 

COMMON/ARR_Gu/Gu(6,3,6) 

COMMON/ ARR_R/ARM(6, 3) 

C PUT Gu IN MATRIX FORM 

C SIMILAR REASON AS IN SUBROUTINE P_LINK FOR THE 
C FOLLOWING STEPS 
J=N+1 

IF(N .EQ. 6)J=1 
Gu(N,l,l)«l. 

Gu(N,l,2)=0. 

Gu(N,l,3)=0. 

Gu(N,l,4)=0. 

Gu(N, 1 ,5)= ARM(J,3) 

Gu(N, 1 ,6)=- ARM(J,2) 

Gu(N,2,l)=0. 

Gu(N,2,2)=l. 

Gu(N,2,3)=0. 

Gu(N,2,4)=-ARM(J,3) 

Gu(N,2,5)=0. 

Gu(N,2,6)= ARM( J, 1 ) 

Gu(N,3,l)=0. 

Gu(N,3,2)=0. 

Gu(N,3,3)=1. 

Gu(N,3,4)=ARM(J,2) 

Gu(N,3,5)=-ARM(J,l) 

Gu(N,3,6)=0. 

RETURN 
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END 

* * * * * * * * * * * * * * * * * * * * * * * * * * * * He * * * * * * * * * * * * * * * * * * * * * ** * * * * * * * * * * * * % * 

* * 

* SUBROUTINE CAL_Huu * 

* * 
** * * * * * * ** * * ** * *** * * ** * ** * **** * * * * * * ******** * * * * * * * ** * ** * * * * * * * * * * 

SUBROUTINE CAL_Huu(N) 

COMMON/ARR_Huu/Huu(6,3,6,6) 

COMMON/ARR_R/ARM(6,3) 

DATA Huu/648*0./ 

C PUT Huu IN MATRIX FORM 

C SIMILAR REASON AS BEFORE 

J=N+1 

EF(N .EQ. 6)J=1 
Huu(N, 1 ,4,5)=ARM(J,2) 

Huu(N, 1 ,4,6)= ARM(J,3) 

Huu(N, 1 ,5,5)=- ARM(J, 1 ) 

Huu(N, 1 ,6,6)=- ARM(J, I ) 

C 

Huu(N,2,4,4)=-ARM(J,2) 

Huu(N,2,4,5)=ARM(J, 1 ) 

Huu(N,2,5,6)=ARM(J,3) 

Huu(N,2,6,6)=-ARM(J,2) 

C 

Huu(N,3,4,4)=-ARM(J,3) 

Huu(N,3,4,6)=ARM(J,1) 

Huu(N,3,5,5)=-ARM(J,3) 

Huu(N,3,5,6)=ARM(J,2) 

RETURN 

END 
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****************************************************************** 
* * 

* SUBROUTINE CAL_G_Hp_phi * 

* * 
****************************************************************** 

SUBROUTINE CAL_G_Hp_phi(N,XP,YP,ZP,ZGp_phi,ZHp_phi) 

REAL L3L 

DIMENSION ZGp_LK(3,3),ZHpp_LK(3, 3,3), ZGp_phi(3, 3), 

+ ZHp_phi(3,3,3) 

C0MM0N/CGpHpp/L3,PSI 1 ,PSI2,S 1 ,S2,C 1 ,C2,T2,TETA 1 , 

+ TETA2 

COMMON/DIRECT/DGp(6,3,3),DHpp(6,3,3,3) 

PI=ACOS(-l.) 

L3=SQRT(XP**2+YP**2+ZP**2) 

PSI2=ACOS(ZP/L3) 

TETA2=PI+PSI2 

L=L3*SIN(PSI2) 

PSI 1 = ASIN(YP/L) 

TETA 1 =PI+PSI 1 
S1=SIN(PSI1) 

S2=SIN(PSI2) 

Cl=COS(PSIl) 

C2=COS(PSI2) 

T2=TAN(PSI2) 

C TWO DIFFERENT WAYS TO DETERMINE Gp_phi AND Hp_phi 

C THIS PROGRAM USED THE RESULTS FROM INDIRECT, BUT 
C THE RESULTS FROM DIRECT SERVED AS A CHECK 

C CALL CAL_DR_Gp(N,XP,YP,ZP,DGp) 

C CALL CAL_DR_Hpp(N,XP,YP,ZP,DGp,DHpp) 

CALL INDIRECT (N,ZGp_phi,ZHp_phi) 

RETURN 

END 
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SUBROUTINE INDIRECT 


SUBROUTINE INDIRECT(N,ZGp_phi,ZHp_phi) 

C THIS SUBROUTINE DETERMINES Gp_phi AND Hp_phi 
C INDIRECTLY 

DIMENSION Gs(3,3),Hss(3,3,3),ZGp_phi(3,3)> 

+ ZHp_phi(3,3,3) 

COMMON/G_Hp_phi/Gp_phi(6,3,3)TIp_phi(6,3,3,3) 

CALL CAL_Gs(N,STl ,ST2,CT1 ,CT2,Gs,ZGp_phi) 

CALL CAL_Hss(N,STl,ST2,CTl,CT2,Gs,Hss,ZHp_phi) 
CALL CHG_DIMLO(N,3,3,ZGp_phi,3,3,3,ZHp_phi,Gp_phi, 
+ Hp_phi) 

RETURN 

END 


SUBROUTINE CAL_Hphi_pp 


♦ * * * * * ** * ****** ***** * * **** *********** **************************** * 


SUBROUTINE CAL_Hphi_pp(N,ZGp_phi,ZHp_phi) 

C THIS SUBROUTINE CALCULATES THE INVERSE OF Gp_phi 
AND Hp_phi 

COMMON/G_Hphi_pp/Gphi_p(6,3,3).Hphi_pp(6,3,3 5 3) 
DIMENSION ZGp_phi(3,3),ZHp_phi(3,3,3),ZGphijp(3,3), 

+ TZGphi_p(3,3),XX(3,3,3),YY(3,3,3),ZZ(3,3,3), 

+ ZHphi_pp(3,3>3) 

CALL MATRIX_INV (3 ,ZGp_phi,ZGphi_p) 

CALL TRANSPOSE(3,3,ZGphi_p,TZGphi_p) 

CALL GENERAL_DOT(3,3,ZGphi_p,3>ZHp_phi,XX) 
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DO 1=1,3 

CALL NNJX)T_NNN(3,3,I,3,TZGphi_p,XX,YY) 

CALL NNN_DOT_NN (3,3,1 ,3 , Y Y,ZGphi_p,ZZ) 

DO J=l,3 
DO K=l,3 

ZHphi_pp(I,J,K)=-ZZ(I,J,K) 

ENDDO 

ENDDO 

ENDDO 

CALL CHG_DIMLO(N,3,3,ZGphi_p,3,3,3,ZHphi_pp,Gphi_p, 

+ Hphi_pp) 

RETURN 

END 

* * * * * * *** * * ** * * * * * * * * * ** * * * * * * * * 3k * ** * * * * * * ********* * * **** ** * * * * * * * 

* * 

* SUBROUTINE CAL_DR_Gp * 

* * 
* * * * * * * * ** * % ** * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ** * * * * dfC * * * * * * * * * 

SUBROUTINE CAL_DR_Gp(N,XP,YP,ZP,Gp) 

C TfflS SUBROUTINE IS NOT ACTUALLY USED BUT ONLY 
C SERVES AS A CHECK 
REAL L3,L 

DIMENSION Gp(6,3,3) 

COMMON/CGpHpp/L3 ,PS 1 1 ,PSI2,S 1 ,S2,C1 ,C2,T2,TETA 1, 

+ TETA2 

A1=(L3**2)*T2 
D2DXP=XP/A1 
D2DYP=YP/A1 

D2DZP=(ZP*C2-L3)/((L3**2)*S2) 

A2=C1*(L3*S2)**2 
D1DXP=-XP*S 1/A2 
D1DYP=(L3*S2-YP*S1)/A2 
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D1DZP=0 

D3DXP=XP/L3 

D3DYP=YP/L3 

D3DZP=ZP/L3 

Gp(N, 1 , 1 )=D 1DXP 

Gp(N,2, 1 )=D2DXP 

Gp(N,3, 1 )=D3DXP 

Gp(N,l,2)=DlDYP 

Gp(N,2,2)=D2DYP 

Gp(N,3,2)=D3DYP 

Gp(N,l,3)=DlDZP 

Gp(N,2,3)=D2DZP 

Gp(N,3,3)=D3DZP 

RETURN 

END 

* * * * * * * * * * * * * * * * * ** * * * * ** ** * *** * ** * ** * *** * *********************** * 
* * 

* SUBROUTINE CAL_DR_Hpp * 

* * 

* * * * * * ** * ****** * * * * * * * * *** * * * * *** * ****** * *** * *** * *** *********** * * * 

SUBROUTINE CAL_DR_Hpp(N,XP,YP,ZP,Gp,Hpp) 

C THIS SUBROUTINE IS ALSO NOT USED BUT SERVES AS A 
C CHECK 
REAL L3L 

DIMENSION Gp(6,3,3),Hpp(6,3,3,3) 

COMMON/CGpHpp/L3 ,PSI 1 ,PSI2,S 1 ,S2,C 1 ,C2,T2,TETA 1 , 

+ TETA2 

A2=C 1 *(L3*S2)**2 
D23D2XP=(L3**2-XP**2)/L3**3 
D23DXPDYP=-XP*YP/L3**3 
D23DXPDZP=-XP*ZP/L3**3 
D23DYPDXP=D23DXPDYP 
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D23D2YP=((L3**2)-(YP**2))/L3**3 

D23DYPDZP=-YP*ZP/L3**3 

D23DZPDXP=D23DXPDZP 

D23DZPDYP=D23DYPDZP 

D23D2ZP=((L3**2)-(ZP**2))/L3**3 

A4=(L3**4)*(T2**3) 

D22D2XP=(((L3*T2)**2)-2*((XP*T2)**2)- 
+ (XP/C2)**2)/A4 

D22DXPDYP=(-XP*YP*(((1/C2)**2)+2*(T2**2)))/A4 
A5=(L3**4)*(T2**2)*S2 
D22DXPDZP=(-2*XP*ZP*S2*T2-XP*ZP/C2 
+ +L3*XP/(C2**2))/A5 

D22D2YP=((L3*T2)**2-2*((YP*T2)**2)-(YP/C2)**2)/A4 
D22DYPDXP=D22DXPDYP 
D22DYPDZP=(-2*YP*ZP*S2*T2-YP*(ZP*C2- 
+ L3)/(C2**2))/A5 

D22DZPDXP=D22DXPDZP 
D22DZPDYP=D22DYPDZP 

D22D2ZP=((S2*L3**2)*(-Gp(N,2,3)*ZP*S2+C2-ZP/L3)- 

(ZP*C2-L3)*(2*ZP*S2+Gp(N,2,3)*C2*L3**2)) 
+ /((S2*L3**2)**2) 

A6=2*C1*S2**2 

A7=XP*C1 

A8=XP*S1 

A9=2*C 1 *C2*S2*L3**2 

A10=S1*(S2*L3)**2 

A11=S1*S2**2 

A12=L3*C2 

A13=L3*S2-YP*S1 

D2 lD2XP=-(A2*(Gp(N, 1 , 1 )* A7+S 1 )-A8*(XP* A6+ 

+ Gp(N,2, 1 )* A9-Gp(N, 1 , 1 )*A10))/A2**2 

D21DXPDYP=-(A2*Gp(N, 1 ,2)*A7-A8*(YP*A6+ 



Gp(N,2,2)*A9-Gp(N,l,2)*A10))/A2**2 
D2 1 DXPDZP=-(A2*Gp(N, 1 ,3)* A7-A8*(ZP*A6+ 

Gp(N,2,3)*A9-Gp(N, 1 ,3)*A10))/A2**2 
D2lD2YP=(A2*(YP*S2/L3+Al2*Gp(N,2,2)-Sl- 

Gp(N, 1 ,2)* YP*C 1 )- A l3*(YP*A6+Gp(N,2,2)*A9- 
Gp(N, 1 ,2)* A 1 0))/A2* *2 
D21DYPDZP=(A2*(ZP*S2/L3+A12*Gp(N,2,3)- 
Gp(N, 1 ,3)* YP*C 1)- A 1 3* (ZP*A6+ 
Gp(N,2,3)*A9-Gp(N, 1 ,3)*A10))/A2**2 

D21D2ZP=0 
D21DZPDXP=0 
D21DZPDYP=0 
D2 1DYPDXP=D2 1DXPD YP 
Hpp(N, 1,1,1 )=D2 1D2XP 
Hpp(N,l,l,2)=D21DXPDYP 
Hpp(N ,1,1,3 )=D2 1 DXP DZP 
Hpp(N,l,2,l)=D21DYPDXP 
Hpp(N , 1 ,2,2)=D2 1 D2 YP 
Hpp(N, 1 ,2,3)=D2 1DYPDZP 
Hpp(N, 1,3,1 )=D2 1DZPDXP 
Hpp(N,l,3,2)=D2 1DZPDYP 
Hpp(N,l,3,3)=D2lD2ZP 

Hpp(N,2,l , 1 )=D22D2XP 
Hpp(N,2, 1 ,2)=D22DXPD YP 
Hpp(N,2, 1 ,3)=D22DXPDZP 
Hpp(N,2,2, 1 )=D22D YPDXP 
Hpp(N,2,2,2)=D22D2YP 
Hpp(N,2,2,3)=D22DYPDZP 
Hpp(N,2,3,l)=D22DZPDXP 
Hpp(N,2,3,2)=D22DZPDYP 
Hpp(N,2,3,3)=D22D2ZP 



173 


* 

Hpp(N,3, 1 , 1 )=D23D2XP 

Hpp(N,3, 1 ,2)=D23DXPDYP 

Hpp(N,3, 1 ,3)=D23DXPDZP 

Hpp(N,3,2, 1 )=D23DYPDXP 

Hpp(N,3,2,2)=D23D2YP 

Hpp(N,3,2,3)=D23DYPDZP 

Hpp(N,3,3 , 1 )=D23DZPDXP 

Hpp(N,3,3,2)=D23DZPDYP 

Hpp(N,3,3,3)=D23D2ZP 

RETURN 

END 

* * * * * * * * * * * * * * * * * % * * * * * * % * * * * * * * * * * * * * * * * * * * ^ * * * % * * * * * * * % * * * * % * * * * 
* * 

* SUBROUTINE CAL J3s * 

* * 

****************************************************************** 

SUBROUTINE CAL_Gs(N,STl ,ST2,CT1 ,CT2,Gs,Gphi) 

C THIS SUBROUTINE CALCULATES THE G-FUNCTION FOR 
C EACH LEG 

REAL L3 

C0MM0N/CGpHpp/L3JPSIl ,PSI2,S 1 ,S2,C 1 ,C2,T2,TETA 1, 

+ TETA2 

COMMON/TETA_UST(6),CT(6),RL3(6) 

DIMENSION Gs(3,3),R(3,3),Gphi(3,3) 

WRTTE(1 *)'ACTUATOR LENGTH L3=',L3 
ST1=SIN(TETA1) 

ST2=SIN(TETA2) 

CT1=C0S(TETA1) 

CT2=COS(TETA2) 

ST(N)=ST2 

CT(N)=CT2 



RL3(N)=L3 

Gs(1,1)=-L3*ST1*ST2 

Gs(l ,2)=L3*CT1 *CT2 

Gs(l,3)=CTl*ST2 

Gs(2,l)=L3*CTl*ST2 

Gs(2,2)=L3*STl*CT2 

Gs(2,3)=STl*ST2 

Gs(3,l)=0 

Gs(3,2)=L3*ST2 

Gs(3,3)=-CT2 

C TRANSFORM THE G-FUNCTION FROM THE LEG FRAME TO THE 
C BASE FRAME 

CALL ROT_MATRIX(N,R) 

CALL MATRIX_MULT(3,3JU,3,Gs,Gphi) 

RETURN 

END 

* sfc* * * * * * * afc * * 5ft ** * * * * * * * ** * * * ***** ****** * * ****** * ** * * * *** 3 jc *** * ** sfc * jjc * 

* * * 

* SUBROUTINE CAL_Hss * 

* * 

SUBROUTINE CAL_Hss(N,STl,ST2,CTl f CT2,Gs,Hss,Hphi) 

C THIS SUBROUTINE CALCULATES THE H-FUNCTION FOR 
C EACH LEG 
REAL L3 

DIMENSION Hss(3,3,3),Gs(3,3),R(3,3),Hphi(3,3,3) 

COMMON/CGpHpp/L3 ,PS 1 1 ,PSI2,S 1 ,S2,C 1 ,C2,T2,TETA 1 , 

TETA2 

Hss(l,l,l)=-Gs(2,l) 

Hss(l,l,2)=-Gs(2,2) 

Hss(1,1,3)=-Gs(2,3) 

Hss(l,2,l)=Hss(l,l,2) 


+ 
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Hss(l,2,2)=Hss( 1,1,1) 

Hss( 1 ,2,3)=CT1 *CT2 
Hss(l,3,l)=Hss(l,l,3) 

Hss( 1 ,3,2)=Hss( 1,2,3) 

Hss(2,l,l)=Gs(l,l) 

Hss(2,l,2)=Gs(l,2) 

Hss(2,l,3)=Gs(l,3) 

Hss(2,2, 1 )=Hss(2, 1 ,2) 

Hss(2,2,2)=Hss(2, 1,1) 

Hss(2,2,3)=STl*CT2 
Hss(2,3, l)=Hss(2, 1,3) 

Hss(2,3,2)=Hss(2,2,3) 

Hss(3,2,2)=L3*CT2 

Hss(3,2,3)=ST2 

Hss(3,3,2)=Hss(3,2,3) 

C TRANSFORM THE H-FUNCTION FROM THE LEG FRAME TO THE 

C BASE FRAME 

CALL ROT_MATRIX(N,R) 

CALL GENERAL_DOT(3,3,R,3,Hss,Hphi) 

RETURN 

END 

* * * * * * * * * * * * * * * * * * % * * * * * * * * * * * * * * * * * * * 5k * 3|e * % * * * * * * * * * * * * * * * * * * * * * ** 

* * 

* SUBROUTINE CAL_G_Hphi_uu * 

* * 

****************************************************************** 

SUBROUTINE CALGHphiuu 

C THIS SUBROUTINE TRANSFERS THE G- AND H-FUNCTIONS 

C TO REFERENCE THEM TO THE COMMON PLATFORM 

C COORDINATES 

DIMENSION ZGu(3,6),ZGp_phi(3,3),ZHuu(3,6,6), 

+ ZHp_phi(3,3,3),ZGphi_u(3,6), 



ZHphi_uu(3,6,6),TGu(6,3),GpHuu(3,6,6), 
TGu_DOT_Hphi(3,6,3),TGuHphiGu(3,6,6), 
ZIGp_phi(3,3),ZGphi_p(3,3),ZHphi_pp(3,3,3) 
COMMON/ ARR_Gu/Gu(6,3, 6) 

COMMON/ARR_Huu/Huu(6, 3,6,6) 
COMMON/G_Hp_phi/Gp_phi(6,3,3),Hp_phi(6,3,3,3) 
COMMON/ARR_phi_uu/Gphi_u(6,3,6),Hphi_uu(6,3,6,6) 
COMMON/G_Hphi_pp/Gphi_p(6,3,3),Hphi_pp(6,3,3,3) 

DO 1=1,6 

SET Gu AND Gp INTO 2-D ARRAY 
SET Huu AND Hpp INTO 3-D ARRAY 
CALL CHG_DEMHI(I,0,Gu,Huu,ZGu,ZHuu) 

CALL CHG_DEMHI(I,3,Gphi_p,Hphi_pp,ZGphi_p,ZHphi_pp) 
CALL TRANSPOSE(3,6,ZGu,TGu) 

CALL MATRIX_MULT(3,3,ZGphi_p,3,6,ZGu,ZGphi_u) 
CALL GENERAL_DOT(3,3,ZGphi_p,6,ZHuu,GpHuu) 

DO J=l,3 

CALL NN_DOT_NNN(6,3,J,3,TGu,ZHphi_pp, 

TGu_DOT_Hphi) 

CALL NNN_DOT_NN(6,3J,3,TGu_DOT_Hphi,ZGu, 

TGuHphiGu) 

CALL ADDT(3,6,J,GpHuu,TGuHphiGu,ZHphi_uu) 

ENDDO 

SET Gphi_u INTO 3-D ARRAY AND Hphi_uu INTO 4-D 
ARRAY 

CALL CHG_DIMLO(I,3,6,ZGphi_u,3,6,6,ZHphi_uu, 
Gphi_uJIphi_uu) 

ENDDO 

RETURN 

END 
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* * 

* SUBROUTINE GET_CONST * 

* * 

SUBROUTINE GET_CONST(Izzl,Izz2,Ixx2,Ixx3,Iyy3, 

+ M23,M34JLc,Lr) 

C TfflS SUBROUTINE CALCULATES THE INERTIA TENSORS FOR 
C EACH LINK 

C REFERENCED TO THEIR RESPECTIVE LOCAL COORDINATES 
REAL Izz 1 ,Izz2,Ixx2,Ixx3,Iyy 3 ,M 12,M23,M34,Lc,Lr 
READ(40,*) M12,M23,M34,R,LcUr 
Izzl=(M12*R**2)/2. 

Izz2=(M23*Lc**2)/12. 

Ixx2=Izz2 

Ixx3=(M34*Lr**2)/12. 

Iyy3=Ixx3 

RETURN 

END 

* * 

* SUBROUTINE CAL_ENERTIA * 

♦ * 

SUBROUTINE CAL_INERTIA(N,Izzl,Izz2,Ixx2,Ixx3 ) Iyy3, 

+ M23,M34,Lc,Lr,Iphi_LK) 

C THIS SUBROUTINE CALCULATES THE EFFECTIVE INERTIA 
C MATRIX FOR EACH LEG EXPRESSED IN THE RESPECTIVE 

C LEG FRAME 

REAL Izzl,Izz2,Ixx2,Ixx3,Iyy3,M23,M34,Lc,Lr, 

+ Iphi_LK(6,3,3) 

COMMON/TET A_L/ST2(6),CT2(6),RL3 (6) 
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Iphi_LK(N,l,l)=Izzl+M23*(Lc*ST2(N)/2.)**2+ 

+ Ixx2*ST2(N)**2+M34*(RL3(N)-Lr/2.)**2* 

+ ST2(N)**2+Ixx3*ST2(N)**2 

Iphi_LK(N,2,2)=M23*(Lc/2.)**2+Izz2+M34*(RL3(N)- 
+ Lr/2.)**2+Iyy3 

Iphi_LK(N,3,3)=M34 
RETURN 
END 

****************************************************************** 
* * 


* SUBROUTINE CAL_POW_phi * 

* * 


****************************************************************** 


SUBROUTINE C AL_POW_phi(N,Ixx2,Ixx3 ,Iyy 3 ,M23 ,M34, 

+ LcLr,Pphi_LK) 

C THIS SUBROUTINE CALCULATES THE INERTIA POWER 
C ARRAY FOR EACH LEG EXPRESSED IN THE RESPECTIVE 

C LEG FRAME 

REAL Ixx2,Ixx3,Iyy3,M23,M34,Lc,Lr 
DIMENSION Pphi_LK(6,3,3»3) 
COMMON/TETA_L/S2(6),C2(6),RL3(6) 

R=RL3(N)-Lr/2. 

SC=S2(N)*C2(N) 

Pphi_LK(N,l,l,2)=M23*SC*(Lc/2.)**2+2.*Ixx2*SC+ 

+ M34*R**2*SC+2.*Ixx3*SC 

Pphi_LK(N,l , 1 ,3)=M34*R*S2(N)**2 
Pphi_LK(N,l>2,l)=M23*SC*(Lc/2.)**2+M34*SC*R**2 
Pphi_LK(N,l>2,3)=-Iyy3*S2(N) 
Pphi_LK(N,l,3,l)=M34*R*S2(N)**2 
Pphi_LK(N,2,l,l)=-M23*SC*(Lc/2.)**2-Ixx2*SC- 
+ M34*SC*R**2-Ixx3*SC 

Pphi_LK(N,2, l,3)=Ixx3*S2(N) 
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Pphi_LK(N,2,2,3)=M34*R 
Pphi_LK(N,2,3,2)=M34*R 
Pphi_LK(N,3, 1 , 1)=-M34*R*S2(N)**2 
Pphi_LK(N,3,l,2)=-Ixx3*S2(N) 

Pphi_LK(N,3,2, l)=Iyy3*S2(N) 

Pphi_LK(N,3,2,2)=-M34*R 

RETURN 

END 

* * * * * * * * * * * * ** * * * * * * * * * * * * * ** * * * * * * * * * * * * * * * * ** * * * * * * * * * * * 94c * * ** ** * 

* * 

* SUBROUTINE CAL_Iuu_Puuu * 

* * 

****************************************************************** 
SUBROUTINE CAL_Iuu_Puuu 

C THIS SUBROUTINE CALCULATES THE TOTAL EFFECTIVE 

C INERTIA MATRIX AND INERTIA POWER ARRAY FOR THE 

C SIX LEGS REFERENCED TO THE COMMON PLATFORM 

C COORDINATES* 

COMMON/ARR_phi_uu/Gphi_u(6,3,6),Hphi_uu(6,3,6,6) 

COMMON/SINERTIA_phi/SIphi_LK(6,3,3) 

COMMON/POWER j>hi/Pphi_LK(6, 3, 3, 3) 

COMMON/SINERTIA_u/SIuu(6,6,6) 

COMMON/POWER_u/Puuu(6,6,6,6) 

COMMON/ARR_Gu/Gu(6,3,6) 

COMMON/ARR_Huu/Huu(6,3,6,6) 

COMMON/G_Hphi_pp/Gphi_p(6,3,3),Hphi_pp(6,3,3,3) 

DIMENSION ZGphi_u(3,6),TGphi_u(6,3),ZHphi_uu(3,6,6), 

+ TZGphi_u(6,3),ZSIphi_LK(3,3), 

+ ZPphi_LK(3,3,3),ZHu_phi(6,3,3),ZSIuu(6,6), 

+ ZPuuu(6,6,6),ZGphi_p(3,3),ZHphi_pp(3,3,3), 

+ TZGphi_pp(3,3),W(3,3),WW(3,6,6),X(3,3,3), 

+ TZGphi_p(3,3),ZGu(3,6),ZHuu(3,6,6),TGu(6,3), 
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+ Y(3,3,3),YY(3,3,3),X_YY(3,3,3),V(3,6,3), 

+ VV(3,6,6),WV(3,6,6) 

DO 1=1,6 

C SET 3-D ARRAY INTO 2-D AND 4-D ARRAY INTO 3-D 

CALL CHG_DIMHI(I,0,Gphi_u,Hphi_uu,ZGphi_u,ZHphi_uu) 
CALL CHG_DIMHI(I,3,SIphi_LK,Pphi_LK,ZSIphi_LK, 

+ ZPphi_LK) 

CALL CHG_DIMHI(I,0, Gu, Huu, ZGu,ZHuu) 

CALL CHG_DIMHI(I,3,Gphi_p,Hphi_pp,ZGphi_p,ZHphi_pp) 
CALL CAL_I(3,6,ZGphi_u,TZGphi_u,3,3,ZSIphi_LK,ZSIuu) 
CALL TRANSPOSE(3,3,ZGphi_p,TZGphi_p) 

CALL MATRIX_MULT(3,3,ZSIphi_LK,3,3,ZGphi_p,W) 
CALL GENERAL_DOT(3 ,3 , W , 6,ZHuu , W W) 

CALL GENERAL_DOT (3,3 ,ZSIphi_LK,3 ,ZHphi_pp,X) 
CALL TRANSPOSED, 6, ZGu,TGu) 

DO J=l,3 

CALL NN_DOT_NNN(3,3,J,3,TZGphi_p,ZPphi_LK,Y) 
CALL NNN_DOT_NN(3,3,J,3,Y,ZGphi_p,YY) 

ENDDO 
DO J= 1,3 

CALL ADDT(3,3,J,X,YY,X_YY) 

CALL NN_DOT_NNN(6,3,J,3,TGu,X_YY,V) 

CALL NNN_DOT_NN(6,3,J,3,V,ZGu,VV) 

CALL ADDT (3 ,6, J,WW, W, W V) 

ENDDO 

CALL GENERAL_DOT(6,3,TZGphi_u,6,WV,ZPuuu) 

CALL CHG_DIMLO(I,6,6,ZSIuu,6,6,6,ZPuuu,SIuu,Puuu) 
ENDDO 
RETURN 
END 
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* ate * * a|c * * * * * ** * * * * a|e * * % * * * * * * * * * * * * * * * * * * * * * * 3k * * ** * * * * * * * * * * * * ** * * ** * 

* * 

* SUBROUTINE CAL_I * 

* * 

****************************************************************** 

SUBROUTINE CAL_I(I,J,A,TA,K,L,B ,C) 

C THIS SUBROUTINE SERVES AS A GENERAL EXPRESSION TO C 
CALCULATE THE EFFECTIVE INERTIA MATRIX 

DIMENSION A(I,J),TA(6,6),B(K,L),C(J,J),TAS(6,6) 

CALL TRANSPOSE(I,J,A,TA) 

CALL MATRIX_MULT(J,I,TA,K,L,B,TAS) 

CALL MATRIX_MULT(J,L,TAS,I,J,A,C) 

RETURN 

END 

* * 

* SUBROUTINE PFORM_INERTIA * 

* '* 

****************************************************************** 
SUBROUTINE PFORM_INERTIA(RR,PL_G,PF_Iuu) 

C THIS SUBROUTINE CALCULATES THE EFFECTIVE INERTIA 
C MATRIX OF THE PLATFORM 

COMMON/TRAN_P_G/R(3,3) 

REALM 

DIMENSION PF_Iuu(6,6),PL_I(3,3),PL_G(3,3), 

+ PP(3,3),TR(3,3) 

M=2250. 

PF_Iuu(l,l)=M 
PF_Iuu(2,2)=M 
PF_Iuu(3,3)=M 
PL_I( 1 , 1 )=(M*RR**2)/4. 

PL_I(2,2)=PL_I( 1,1) 
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PL_I(3,3)=(M*RR**2)/2. 

C TRANSFORM Ixx,Iyy AND Izz TO GLOBAL COOR 
CALL MATRIX_MULT(3,3,R,3,3,PL_I,PP) 

CALL TRANSPOSE(3,3,R,TR) 

CALL MATRIX_MULT(3,3,PP,3,3,TR,PL_G) 

DO K=l,3 
DO J-1,3 

PF_Iuu(K+3,K+3)=PL_G(K,J) 

ENDDO 

ENDDO 

RETURN 

END 

****************************************************************** 
* * 

* SUBROUTINE PFORM_INEPOW * 

* * 
***************************************** *************** ********** 

SUBROUTINE PFORM_INEPOW(P_luu,P_uuu) 

C THIS SUBROUTINE CALCULATES THE INERTIA POWER 
C ARRAY OF THE PLATFORM 

DIMENSION P_luu(3,3),P_uuu(6,6,6) 

DO 1=4,6 
J=I-3 

P_uuu(4,I,5)=P_luu(J,3) 

P_uuu(4,I,6)=-P_luu(J,2) 

P_uuu(5,I,4)=-P_luu(J,3) 

P_uuu(5,I,6)=P_luu( J, 1 ) 

P_uuu(6,I,4)=P_luu(J,2) 

P_uuu(6,I,5)=-P_luu(J,l) 

ENDDO 

RETURN 

END 
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; i83 

* * * * * ******* * ** * * * * * ** * * * * * ★ ate ** * * ** * * * * * * * * 94c * * % * * * * * *** * sic * * * * * * * * * 

* * 

* SUBROUTINE I_STAR_uu * 

* * 

| ****************************************************************** 

| SUBROUTINE I_STAR_uu(PF_Iuu,TOT_Iuu) 

C THIS SUBROUTINE CALCULATES THE EFFECTIVE INERTIA 
C MATRIX OF THE GENERALIZED STEWART PLATFORM 

| C REFERENCED TO THE PLATFORM 

, COMMON/SINERTIA_u/SIuu(6,6,6) 

l DIMENSION STOT_Iuu(6,6),PF_Iuu(6,6),TOT_Iuu(6,6) 

j DO 1=1,6 

1 DO J=l,6 

STOT_Iuu(I,J)=0. 

1 DO K-1,6 

, STOT_Iuu(I,J)=SIuu(K,I,J)+STOT_Iuu(I,J) 

ENDDO 

j TOT_Iuu(I,J)=STOT_Iuu(U)+PF_Iuu(U) 

\ ENDDO 

| ENDDO 

i RETURN 

! END 

* * *** * **** * *********** afe ** * ****** * *** * **** * * ************ ********** * 

* * 

* SUBROUTINE P_STAR_uuu * 

* * 

****************************************************************** 

SUBROUTINE P_STAR_uuu(PF_Puuu,TOT_Puuu) 

C THIS SUBROUTINE CALCULATES THE INERTIA POWER 
C ARRAY OF THE GENERALIZED STEWART PLATFORM 

C REFERENCED TO THE PLATFORM 

COMMON/POWER_u/Puuu(6,6,6,6) 



DIMENSION PF_Puuu(6,6,6),STOT_Puuu(6,6,6), 

+ TOT_Puuu(6,6,6) 

DO 1=1,6 
DO J=l,6 
DO K=l,6 

STOT_Puuu(I,J,K)=0. 

DO L=l,6 

STOT_Puuu(I,J,K)=Puuu(L,I,J,K)+STOT_Puuu(I,J,K) 

ENDDO 

TOT_Puuu(I,J,K)=STOT_Puuu(I,J,K)+PF_Puuu(I,J,K) 

ENDDO 

ENDDO 

ENDDO 

RETURN 

END 

* * 

* SUBROUTINE CAL_Gq_u * 

* * 
♦♦JO*************************************************************** 

SUBROUTINE CAL_Gq_u 

C THIS SUBROUTINE DOES THE EXTRACTION OF THE 
C G-FUNCTION REFERENCED TO THE DESIRED JOINT SET OF 

C COORDINATES 

COMMON/ARR_phi_uu/Gphi_u(6,3,6),Hphi_uu(6,3,6,6) 

COMMON/ARR_Gq_u/Gq_u(6,6) 

WRITE( 1 , * ) 'Gq_u ' 

DO 1=1,6 
DO J=l,6 

Gq_u(I,J)=Gphi_u(I,3,J) 

ENDDO 

WRITE( 1 ,99)(GQ_U(I,J),J= 1 ,6) 
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ENDDO 

99 FORMAT(X,6(F10.3,2X)) 

RETURN 

END 

* * ******* * * * * * ** * * ** * ******* Hi ******************* ***** * * * * He * * ** * ** * 

* * 

* SUBROUTINE CAL_Hq_uu * 

* * 
**** * ** ***** ** ** * * **** ** * * ***** * * * ** * ik******* **** **** ***** * *** *** * 

SUBROUTINE CAL_Hq_uu 

C THIS SUBROUTINE DOES THE EXTRACTION OF THE 
C H-FUNCTION REFERENCED TO THE DESIRED JOINT SET OF 

C COORDINATES 

COMMON/ARR_phi_uu/Gphi_u(6,3,6),Hphi_uu(6,3,6,6) 

COMMON/ARR_Hq_uu/Hq_uu(6,6,6) 

WRITE(l,*)'Hq_uu' 

DO 1=1,6 

WRITE( 1 ,*)'PLANE=’,I 
DO J=l,6 
DO K=l,6 

Hq_uu(IJ,K)=Hphi_uu(I,3J,K) 

ENDDO 

WRITE(1,99)(FIQ_UU(I,J,K),K= 1,6) 

ENDDO 

ENDDO 

99 FORMAT(X,6(F10.3,2X)) 

RETURN 

END 
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****************************************************************** 
^ j | e 

* SUBROUTINE CAL_Iqq_Pqqq * 

* * 
*** * ** * *** ** ********** * * * ** * ** * * * * * * * * ***** *** * * * * * * * lit * * * * * ***** * * 

SUBROUTINE CAL_Iqq_Pqqq 

C THIS SUBROUTINE CALCULATES THE EFFECTIVE INERTIA 
C MATRIX AND INERTIA POWER ARRAY REFERENCED TO 
C THE DESIRED JOINT SET OF GENERALIZED COORDINATES 

COMMON/ARR_Gq_u/Gq_u(6,6) 

COMMON/ARR_Hq_uu/Hq_uu(6,6,6) 

COMMON/Iuu/TOT_Iuu(6 ,6) 

COMMON/Puuu/TOT_Puuu(6,6,6) 

COMMON/Iqq/STAR_Iqq(6,6) 

COMMON/Pqqq/P_STAR_qqq(6,6,6) 

DIMENSION Gu_q(6,6),TGu_q(6,6),EE(6,6,6),FF(6,6,6), 

+ GG(6,6,6),XX(6,6,6),YY(6,6,6),Hu_qq(6,6,6), 

+ F(6,6,6) 

CALL MATRIX_INV(6,Gq_u,Gu_q) 

CALL CAL_I(6,6,Gu_q,TGu_q,6,6,TOT_Iuu,STAR_Iqq) 
WRITE(l,*)'I*qq' 

DO 11=1,6 

WRITE( 1 ,*)(STAR_IQQ(II,JJ),JJ= 1 ,6) 

ENDDO 

CALL CAL_Hu_qq(Gu_q,TGu_q,Hu_qq) 

CALL GENERAL_DOT(6,6,TGu_q,6,TOT_PuuuJEE) 

CALL GENERAL_DOT (6,6,STAR_Iqq,6,Hq_uu,FF) 

WRITE( 1 ,*)’P*qqq' 

DO 1=1,6 

WRITE( 1 ,*)'PLANE=',I 

CALL SUBT (6,I,EE,FF,GG) 

CALLNN_DOT_NNN(6,6,I,6,TGu_q,GG,XX) 
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CALL NNN_DOT_NN(6,6,I,6,XX,Gu_q,P_STAR_qqq) 

DO J=l,6 . 

WRITE(1,*)(P_STAR_QQQ(I,J,K),K= 1 ,6) 

ENDDO 

ENDDO 

RETURN 

END 

* * 

* SUBROUTINE CAL_Hu_qq * 

★ . 

* 

SUBROUTINE C AL_Hu_qq(G u_q,TG u_q,Hu_qq) 

C THIS SUBROUTINE CALCULATES THE DIRECT TRANSFER OF 
C Gq_u AND Hq_uu 

COMMON/ ARR_Hq_uu/Hq_uu(6,6, 6) 

DIMENSION Gu_q(6,6),TGu_q(6,6),GH(6,6,6),BB(6,6,6), 

+ CC(6,6,6),Hu_qq(6,6,6) 

CALL GENERAL_DOT(6,6,Gu_q,6,Hq_uu,GH) 

DO 1=1,6 

CALL NN_DOT_NNN(6,6,I,6,TGu_q,GH,BB) 

CALL NNN_DOT_NN(6,6,I,6,BB,Gu_q,CC) 

DO J=l,6 
DO K=l,6 

Hu_qq(I,J,K)=-CC(I,J,K) 

ENDDO 

ENDDO 

ENDDO 

RETURN 

END 
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* * 

* SUBROUTINE CHG_DIMHI * 

* * 

SUBROUTINE CHG_DIMHI(M,N,A,B,C,D) 

C THIS SUBROUTINE CHANGES 3-DIMENSIONAL ARRAY TO 
C 2-DIMENSIONAL MATRIX AND 4-DIMENSIONAL ARRAY TO 

C 3-DIMENSIONAL ARRAY 

DIMENSION A(6,3,6-N),B(6,3,6-N,6-N),C(3,6-N), 

+ D(3,6-N,6-N) 

DO J— 1,3 
DO K=l,6-N 
C(J,K)=A(M,J,K) 

DO L=l,6-N 

D(J,K,L)=B(M,J,K,L) 

ENDDO 

ENDDO 

ENDDO 

RETURN 

END 

* * * sjc * * * * * * * * * ***************** * * *** 34c * * *** * * ** sic ** * * * * * * * ** * ** * * * * * * 

* * 

* SUBROUTINE CHG DIMLO * 

* * 
He **** * * * * * * sic * * * * *** * ***** * ** * ************* * ***** * **************** * 

SUBROUTINE CHG_DIMLO(II,JJ,KK,A,LL,MM,NN,B,C,D) 

C THIS SUBROUTINE CHANGES 2-DIMENSIONAL MATRIX TO 
C 3-DIMENSIONAL ARRAY AND 3-DIMENSIONAL ARRAY TO 

C 4-DIMENSIONAL ARRAY 

DIMENSION A(JJ,KK),B(LL,MM,NN),C(6,JJ,KK),D(6,LL,MM,NN) 
DO J=1,JJ 


c 
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DO K=1,KK 
C(II,J,K)=A(J,K) 

DO L=1,NN 

D(II,J,K,L)=B(J,K,L) 

ENDDO 

ENDDO 

ENDDO 

RETURN 

END 

****************************************************************** 
* * 

* SUBROUTINE MATRIXJNV * 

* * 

******** ********************************************************** 

SUBROUTINE MATRIXJNV(N,A,AI) 

C THIS SUBROUTINE FINDS THE INVERSE OF ANY SQUARE N 
C BY N MATRIX 

REAL A(N,N),AI(N,N) 

DIMENSION INTER(15,2) 

C 

DO 1=1, N 
DO J=1,N 
AI(J,I)=A(J,I) 

ENDDO 

ENDDO 

C 

DO 12 K=1,N 
JJ=K 

IF (K.NE.N) THEN 
KP1=K+1 

BIG=ABS(AI(K,K)) 

DO 5 I=KP1,N 



AB=ABS(AI(I,K)) ! SEARCHING FOR LARGEST PIVOT 
IF (BIG.LT.AB) THEN 
BIG=AB 
JJ=I 
END IF 
CONTINUE 
ENDEF 

INTER(K,1)=K 
INTER(K,2)=JJ 
IF (JJ.NE.K) THEN 
DO 8 J=1,N 
TEMP=AI(K,J) 

AI(K,J)=AI(JJ,J) ! INTERCHANGE ROWS 
AI(JJ,J)=TEMP 
CONTINUE 
ENDEF 

DO 10 J=1,N 
IF (J.NE.K) THEN 
TEMP=AI(K,J)/AI(K,K) 

AI(K,J)=TEMP 
ENDIF 
CONTINUE 
TEMP= 1 ./ AI(K,K) 

AI(K,K)=TEMP 
DO 11 I=1N 
IF (I.NE.K) THEN 
DO J=1,N 
IF (J.NE.K) THEN 
TEMP= AI(I, J)- AI(K, J) * AI(I,K) 

AI(I,J)=TEMP 
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ENDIF 

ENDDO 

ENDIF 

11 CONTINUE 
C 

DO 12 1=1 ,N 
IF (I.NE.K) THEN 
TEMP=-AI(I,K)*AI(K,K) 

AI(I,K)=TEMP 

ENDIF 

12 CONTINUE 
DO 13 L=1,N 

K=N-L+1 

KROW =INTER(K, 1 ) 

IROW=INTER (K,2) 

IF (KROW.NE.IROW) THEN 
DO 1=1, N 

TEMP=Al(I,KROW) 

AI(I,KROW)=AI(I,IROW) 

AI(I,IROW)=TEMP 

ENDDO 

ENDIF 

13 CONTINUE 
RETURN 
END 

* * 

* SUBROUTINE GENERAL_DOT * 

* * 

****************************************************************** 
SUBROUTINE GENERAL_DOT(N,NC,A,M,B,AB) 

C THIS SUBROUTINE PERFORMS THE GENERALIZED DOT 
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C PRODUCT OF THE N BY NC MATRIX A AND NC BY M BY M 

C ARRAY B 

DIMENSION AB(N,M,M),A(N,NC),B(NC,M,M) 

DO 1=1, N 
DOJ=lM 
D0K=1M 
AB(I,J,K)=0 
DO L=1,NC 

AB(U,K)=A(I,L)*B(LJ,K)+AB(I,J,K) 

ENDDO 

ENDDO 

ENDDO 

ENDDO 

RETURN 

END 

************************************************* ***************** 
* * 

* SUBROUTINE TRANSPOSE * 


****************************************************************** 


SUBROUTINE TRANSPOSE(M,N,A,AT) 

C THIS SUBROUTINE FINDS THE TRANSPOSE OF THE M B Y N 

C MATRIX A 

DIMENSION A(M,N),AT(NM) 

DOI=lM 
DO J=1,N 
AT(J,I)=A(U) 

ENDDO 

ENDDO 

RETURN 

END 
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* * * * * * * * * * * * * * * * * * * a|c * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * sic * * * * 


* * 

* SUBROUTINE MATRIX_MULT * 

* * 

****************************************************************** 


C 

C 


SUBROUTINE MATRIX JvfULT(IA,J,T,K,L,F,TF) 

THIS SUBROUTINE MULTIPLIES A LA BY J MATRIX WITH A 
K BY L MATRIX F 
DIMENSION T(IA,J)P(KL),TF(IA,L) 

INTEGER Q 
DOM=l,IA 
DO N=1,L 
TF(M,N)=0.0 
DO Q=1,K 

TF(MNO=T(M,Q)*F(Q,N)+TF(M,N) 

ENDDO 

ENDDO 

ENDDO 

RETURN 

END 


****************************************************************** 


* 

* 

* 


* 

SUBROUTINE NNN_DOT_NN * 

* 


****************************************************************** 


SUBROUTINE NNN_DOT_NN (M,N,L,LL, A,B , A_DOT_B ) 

C THIS SUBROUTINE MULTIPLIES ALL BY M BY N ARRAY A 

C WITH A N BY M MATRIX B 

DIMENSION A(LL,M,N) ) B(N,M),A_DOT_B(LL,M,M) 
DOI=lM 
DO J=1,M 
A_DOT_B(L,I,J)=0. 


DO K=1,N 

A_DOT_B(L,U)=A(L,I,K)*B(K,J)+A_DOT_B(L,I,J) 

ENDDO 

ENDDO 

ENDDO 

RETURN 

END 

* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ** * * * * * * * **************** * *** * 


* 


* 


* 

* 


SUBROUTINE NN_DOT_NNN * 

* 


* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
SUBROUTINE NN_DOT_NNN (M,N,L,LL,A,B , A_DOT_B ) 

C THIS SUBROUTINE MULTIPLIES A M BY N MATRIX A WITH A 
C LL BY N BY N ARRAY B 

DIMENSION A(M,N),B(LL,N,N),A_DOT_B(LL,M,N) 

DO 1=1, M 
DO J-1.N 

A_DOT_B(L,I,J)=0. 

DO K=1,N 

A_DOT_B(L,I, J)= A (I,K) *B (L,K, J)+ A JDOT_B (L,I, J) 

ENDDO 

ENDDO 

ENDDO 

RETURN 

END 


* * 

* SUBROUTINE ADDT * 

* * 

************************ ***************************** ************* 


SUBROUTINE ADDT(L,M,N,A,B,A_B) 
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C THIS SUBROUTINE ADDS A L BY M BY M ARRAY A TO A 

C L BY M BY M ARRAY B 

DIMENSION A(LM,M),B(LM,M),A_B(LM,M) 

DO I=1M 
DO J=1M 

A_B(N,I,J)=A(N,I,J)+B(N,I,J) 

ENDDO 

ENDDO 

RETURN 

END 

* * * * * * ** * * * * ** ** * * * * aft * * ** * * a|e * * 34c afc * * a|e * afc afe afc * * * * * afc * * * afe afc ** % * * * * * % a|e dfc a{< 9 |c * * * 

* * 

* SUBROUTINE SUBT * 

* Ac 
****************************************************************** 

SUBROUTINE SUBT(M,N,A,B,A_B) 

C THIS SUBROUTINE SUBSTRACT AN BY M BY M ARRAY A 
C FROM A N BY M BY M ARRAY B 

DIMENSION A(6,6,6),B(6,6,6),A_B(6,6,6) 

DO I=1M 
DO J=1M 

A_B(N,U)=A(N,I,J)-B(N,U) 

ENDDO 

ENDDO 

RETURN 

END 

****************************************************************** 

* * 

* SUBROUTINE ROT_MATRIX * 

* * 

****************************************************************** 
SUBROUTINE ROT_M ATRIX(N,R) 
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C THIS SUBROUTINE ROTATES THE LEG FRAMES TO THE BASE 

C FRAME 

DIMENSION R(3,3) 

DELTA=5. 

IF(N .EQ. 1) AL=0. 

IF(N .EQ. 2) AL=DELTA 
EF(N .EQ. 3) AL=120. 

IF(N .EQ. 4) AL=1 20. + DELTA 
EF(N .EQ. 5) AL=240. 

IF(N .EQ. 6) AL=240. + DELT A 
R(l,l)=l. 

R(2,2)=COSD(AL) 

R(2,3)=-SIND(AL) 

R(3,2)=-R(2,3) 

R(3,3)=R(2,2) 

RETURN 

END 

******** ******************** ************************************** 
* * 

* SUBROUTINE TRANSFPG * 

* * 
****************************************************************** 

SUBROUTINE TRANSF P G 

C THIS SUBROUTINE ROTATES THE PLATFORM FRAME TO THE 
C BASE FRAME 

COMMON/TRAN_P_G/R(3,3) 

COMMON/ORJDEN/X 1 ,Y 1 ,Z 1 ,X 1 2, Y 1 2,Z 1 2 
R(1,1)=X12 

R(1,2)=Y1*Z12-Z1*Y12 

R(1,3)=X1 

R(2,1)=Y12 

R(2,2)=Z1*X12-X1*Z12 
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R(2,3)=Y1 

R(3,1)=Z12 

R(3,2)=X1*Y12-Y1*X12 

R(3,3)=Z1 

RETURN 

END 

* * * * * * ** * * * * * * * * * * ** * ** * * * * ** * ** * * * 3k * * * * * * * * * * * * * * * * * * % * * 3k * * * * * * * * 


* * 

* SUBROUTINE CAL_QDOT * 

* * 
* * * * * * * * * * * * * * * * % * * * ** 3k ********* * ****** * * * * *** 5k * * * **** % ***** 5k *** * * 

SUBROUTINE CAL_QDOT(UDOT,QDOT) 

C THIS SUBROUTINE COMPUTES THE DESIRED ACTUATORS 
C VELOCITY 

COMMON/ARR_Gq_u/Gq_u(6,6) 

DIMENSION UDOT(6,l),QDOT(6,l) 

CALL MATRIX_MULT (6,6,Gq_u,6, 1 ,UDOT,QDOT) 

RETURN 

END 

** * * * * ** % *** * * * * * * * ** * * * * * * * * * * ** * * jft * * * * * * * * * * * * * * * ** * * * % ** * * % a|c * * j|c 


* * 

* SUBROUTINE CAL_QDDOT * 

* * 

****************************************************************** 


SUBROUTINE C AL_QDDOT (UDOT,UDDOT,QDDOT) 

C THIS SUBROUTINE COMPUTES THE DESIRED ACTUATORS 

C ACCELERATION 

COMMON/ARR_Gq_u/Gq_u(6,6) 

COMMON/ ARR_Hq_uu/Hq_uu(6, 6,6) 

DIMENSION UDOT(6, 1 ),UDDOT(6, 1 ),QDDOT(6, 1 ),XX(6, 1 ), 

+ TUDOT( 1 ,6), YY (6, 1 ,6),ZZ(6, 1,1) 

CALL MATRIX_MULT(6,6,Gq_u,6, 1 ,UDDOT,XX) 
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CALL TRANSPOSE^, 1 ,UDOT,TUDOT) 

DO 1=1,6 

CALL NN_DOT_NNN( 1 ,6,I,6,TUDOT,Hq_uu,YY) 

CALL NNN_DOT_NN( 1 ,6,1,6, YY,UDOT,ZZ) 

QDDOT(1, 1 )=XX(1, 1 )+ZZ(1, 1,1) 

ENDDO 

RETURN 

END 

********** sK******************************************************* 
* * 

* SUBROUTINE GEN_FORCES * 

* * 
********************»»**^^c***^^^*^^^* + ^ + ^^^^^ # ^^^ #J)(1(tNtJ(t%J|c#J)ts( . s)t:jt)(ts)ii(<j(<!)c 

SUBROUTINE GEN_FORCES(QDOT,QDDOT) 

C THIS SUBROUTINE COMPUTES THE REQUIRED ACTUATOR 
C FORCES TO CAUSE THE DESIRED PLATFORM MOTIONS 

COMMON/Iqq/STAR_Iqq(6,6) 

COMMON/Pqqq/P_STAR_qqq(6,6,6) 

DIMENSION QDOT(6, 1 ),TQDOT( 1 ,6),QDDOT(6, 1 ),XX(6, 1 ), 

+ TQ(6, 1 ), YY (6, 1 ,6),ZZ(6, 1,1) 

CALL TRANSPOSE(6, 1 ,QDOT,TQDOT) 

CALL MATRIX_MULT(6,6,STAR_Iqq,6, 1 ,QDDOT,XX) 

DO 1=1,6 

CALL NN_DOT_NNN( 1 ,6,I,6,TQDOT,P_STAR_qqq,YY) 

CALL NNN_DOT_NN( 1 ,6,I,6,YY,QDOT,ZZ) 
Tq(I,l)=ZZ(I,l,l)+XX(I,l) 

ENDDO 

WRITE(1,88) 

DO 1=1,6 

WRITE( 1 ,99)I,XX(I,1),ZZ(1, 1 , 1),TQ(1, 1 ) 

ENDDO 

88 FORMAT(/,X,'LINK',10X,'ACC. TERMS', 10X,'VEL. 



TERMS’, lOX/Tq’) 

FORM AT(3X,1 1 , 8X,E 1 5 . 8 ,5X,E 1 5 . 8 ,4X,E 1 5 . 8 ) 

RETURN 

END 



APPENDIX C 


INPUT DATA FOR THE SIMULATION 
PROGRAM IN APPENDIX B 


1 , 0 , 0 , 0 , 1 , 0 , 0 

3 , 0,0 

1 , 0 , 0 , 0 , 0 . 5 , - 0 . 866 , 60 

4 , 0,0 
2,3 

10 , 200 , 175 , 0 . 06 , 3 , 3 
2 
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no 


APPENDIX D 


SUBROUTINE FOR CLASS P=2, 
CONSTANT ACCELERATION 


************************ ********* ********************** ********* 


* 

* 

* 


SUBROUTINE NEXT MOTION 


* 

* 

* 


****************** ********************************************** 


SUBROUTINE NEXT MOTION(TMAX,PTIME,XUi,XUf,THETAi, 

THETAf, XU, THETA, V,W, ACC, 
AL,Y12,Z12) 

THIS SUBROUTINE DETERMINES THE PLATFORM MOTIONS 
USING CLASS P-2, CONSTANT ACCELERATION 
AUG=(PTTME-TMAX/2) 

T=TMAX**2 
IF (AUG .LT. 0)THEN 
AUG=0. 

DEL=0. 

ELSE 

DEL=1. 

ENDIF 

X=XUf-XUi 

TTA=THETAf-THETAi 

XU=XUi+2.*X*(PTIME**2-2.*AUG**2)/T 

V=4.*X*(PTIME-2.*AUG)/T 

ACC=4 *X*(1 -2.*DEL)/T 

THETA=THETAi+2.*TTA*(PTIME**2-2.*AUG**2)/T 

W=4.*TTA*(PTIME-2.*AUG)/T 

AL=4.*TTA*( l.-2.*DEL)/T 

Y12=COS(THETA) 

Z12=SIN(THETA) 

THET A=THET A * 1 80./3. 1415926 

RETURN 

END 
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uu 


APPENDIX E 


SUBROUTINE FOR CLASS P=4, 
4-5-6-7 POLYNOMIAL 


****************************************************************** 
* * 

* SUBROUTINE NEXT_MOTION * 

* * 
****************************************************************** 

SUBROUTINE NEXT_MOTION(TMAX,PTIME,XUi,XUf,THETAi, 

+ THETAf, XU, THETA, V,W, ACC, 

+ AL,Y12,Z12) 

THIS SUBROUTINE DETERMINES THE PLATFORM MOTIONS 
USING CLASS P=4, 4-5-6- 7 POLYNOMIAL 
X=XUf-XUi 
TTA=THETAf-THETAi 
T=PTIME/TMAX 

XU=XUi+X*(35.*(T*M)-84.*(T**5)+70.*(T**6)-20.*(T**7)) 
V=(X/TMAX)*(140.*(T**3)-420.*(T**4)+420.*(T**5)-140.*(T**6)) 
ACC=(X7TMAX**2)*(420.*(T**2)-1680.*(T**3)+2100.*(T**4)- 
840.*(T**5)) 

THETA=THETAi+ITA*(35.*(T*M)-84.*(T**5)+70.*(T**6)- 
20.*(T**7)) 

W=(TTA/TMAX)*(140.*(T**3)-420.*(T**4)+420.*(T**5)-140.*(T**6)) 
AL=(TTA/TMAX**2)*(420.*(T**2)-1680.*(T**3)+2100.*(T**4)- 
840.*(T**5)) 

Y12=C0S(THETA) 

Z12=SIN(THETA) 

THETA=THETA* 1 80./3. 1415926 
RETURN 
END 


202 



APPENDIX F 

SUBROUTINE FOR CLASS P=4, 
THIRD DERIVATIVE TRAPEZOIDAL 


**** ************************************************************** 
* * 

* SUBROUTINE NEXT MOTION * 

* * 
****************************************************************** 

SUBROUTINE NEXT MOTION(TMAX,PTIME,XUi,XUf,THETAi, 

+ " THETAf,XU,THETA,V,W,ACC,AL, 

+ Y12 Z12 T A1 A2,A3) 

C THIS SUBROUTINE DETERMINES THE PLATFORM MOTION S 

C USING CLASS P=4, THIRD DERIVATIVE TRAPEZOIDAL 

DIMENSION T (0: 8),AUG(0:7), Y (2, 4), A 1 (2),A2(2), A3(2) 

DO 11=1,2 
DO 1=4,2,- 1 
DO J=l,8 

AUG(J- 1 )=PTTME-T (J- 1 ) 

EF (AUG(J-l) .LT. 0.)AUG(J-1)=0. 

AUG(J- 1 )= AUG( J- 1 )**I 
ENDDO 
FACT=I 
DO K=I- 1 , 1 ,- 1 
FACT =FACT*K 
ENDDO 

Y(II,I)=(A 1 (Il)/FACT)*((AUG(0)-AUG( 1 ))/(T( 1 )-T(0))- 
+ (AUG(2)-AUG(3))/(T(3)-T(2)))+(A2(II)/FACT)* 

+ ((AUG(2)-AUG(3))/(T(3)-T(2))-(AUG(4)-AUG(5))/ 

+ (T(5)-T(4)))+(A3(II)/FACT)*((AUG(4)-AUG(5)) 

+ / (T (5)-T (4))-(AUG(6)- AUG(7))/ (TMAX-T (6))) 

ENDDO 

ENDDO 

XU=XUi+Y(l,4) 

V=Y(1,3) 

ACC=Yn 2^ 

THETA=THETAi+ Y (2,4) 

W=Y(2,3) 

AL=Y(2,2) 

Y12=COS(THETA) 

Z12=SIN(THETA) 

THETA=THETA*l80./3. 14 15926 

RETURN 

END 
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APPENDIX G 


SUBROUTINE TO DETERMINE THE 
CONSTANTS Al, A2 AND A3 


****************************************************************** 


* 

* 

* 


SUBROUTINE CAL A1A2A3 


* 

* 

* 


****************************************************************** 


SUBROUTINE CAL_A1A2A3(T,A 1 ,A2,A3,XUi,XUf,THETAi, 

+ THETAf) 

C THIS SUBROUTINE DETERMINES THE THREE CONSTANTS A 1 ,A2 
C AND A3 FOR THE CLASS P=4, THIRD DERIVATIVE 

C TRAPEZOIDAL 

DIMENSION T(0:8),A1(2),A2(2),A3(2),TT(3,3),TTI(3,3),Z(3),A(3) 

DO 1=1,3 
DO J=l,3 

TT(I,J)=((((T(7)-T(2*J-2))**(I+1))-(T(7)-T(2*J-1))**(I+1)) 

+ /(T(2*J-l)-T(2*J-2)))-((((T(7)-T(2*J))**(I+l))- 

+ (T(7)-T(2*J+ 1 ))**(I+ 1 ))/(T(2*J+ 1 )-T(2* J))) 

ENDDO 
ENDDO 
DO K=l,2 
IF (K EQ. 1)THEN 
Y=XUf-XUi 
ELSE 

Y=THETAf-THETAi 
END IF 
Z(3)=24.*Y 

CALL MATRIX_1NV (3,TT,TTI) 

CALL MATRDC_MULT(3,3,TTI,3,1,Z,A) 

A1(K)=A(1) 

A2(K)=A(2) 

A3(K)=A(3) 

WRITE(1,*) , A1=',A1(K),'A2=',A2(K),'A3=',A3(K) 

ENDDO 

RETURN 

END 
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